chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add two new attributes to weld constraints:

Browse Source 
- `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
This commit is contained in:
Alessio Quaglino
2022-08-22 08:18:43 -07:00
committed by Copybara-Service
parent f16a6e513d
commit 8ca5887c20
20 changed files with 491 additions and 249 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference.rst
+1 -1
View File
@@ -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 |
doc/XMLreference.rst
+14 -2
View File
@@ -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 <CActRange>`
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"`
doc/changelog.rst
+9 -2
View File
@@ -15,8 +15,13 @@ General
- Added :ref:`adhesion actuators<adhesion>` mimicking vacuum grippers and adhesive biomechanical appendages.
- Added related `example model <https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_ 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 <equality-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 <https://github.com/deepmind/mujoco/tree/main/test/engine/testdata/weld.xml>`_ 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.
include/mujoco/mjmodel.h
+1 -1
View File
@@ -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
src/engine/engine_core_constraint.c
+25 -20
View File
@@ -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; j<NV; j++) {
// axis = [jac0-jac1]_col(j)
axis[0] = jac[0][3*NV+j];
@@ -440,7 +433,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// apply formula
mju_mulQuatAxis(quat2, quat1, axis); // quat2 = neg(q1)*(jac0-jac1)
mju_mulQuat(quat3, quat2, quat); // quat3 = neg(q1)*(jac0-jac1)*q0*data
mju_mulQuat(quat3, quat2, quat); // quat3 = neg(q1)*(jac0-jac1)*q0*relpose
// correct Jacobian
jac[0][3*NV+j] = 0.5*quat3[1];
@@ -448,6 +441,10 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
jac[0][5*NV+j] = 0.5*quat3[3];
}
// scale rotational jacobian by tfratio factor
mjtNum tfratio = data[10];
mju_scl(jac[0]+3*NV, jac[0]+3*NV, tfratio, 3*NV);
size = 6;
break;
@@ -1054,8 +1051,16 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int
case mjCNSTR_EQUALITY:
if (m->eq_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);
src/engine/engine_core_smooth.c
+38 -69
View File
@@ -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; i<nbody; i++)
if (!mju_isZero(d->xfrc_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; i<m->nbody; 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);
src/engine/engine_setconst.c
+13 -10
View File
@@ -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);
}
}
src/engine/engine_vis_visualize.c
+13 -15
View File
@@ -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; i<m->neq; 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
}
}
src/user/user_objects.cc
+6
View File
@@ -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);
}
}
}
src/user/user_objects.h
+2 -2
View File
@@ -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
src/xml/xml_native_reader.cc
+7 -3
View File
@@ -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:
src/xml/xml_native_writer.cc
+3 -1
View File
@@ -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:
test/engine/engine_core_smooth_test.cc
+69 -122
View File
@@ -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);
}
test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml
Vendored
+43
View File
@@ -0,0 +1,43 @@
<!-- Copyright 2021 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco>
<!--
body2 is held by body1 through the weld constraint, so the force sensor on body1
should register -2*gravity (for the weight of both bodies).
-->
<option gravity="1 2 3"/>
<default>
<!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
<geom density="1000" size="0.05 0.05 0.05" type="box"/>
</default>
<worldbody>
<body name="body1">
<geom/>
<site name="sensor"/>
</body>
<body name="body2" pos="1 2 3">
<joint type="free"/>
<geom/>
</body>
</worldbody>
<equality>
<weld body1="body1" body2="body2" anchor="0 0 0" tfratio="0"/>
</equality>
<sensor>
<force site="sensor" user="-2 -4 -6"/>
</sensor>
</mujoco>
test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml
Vendored
+43
View File
@@ -0,0 +1,43 @@
<!-- Copyright 2021 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco>
<!--
body2 is held by body1 through the weld constraint, so the force sensor on body1
should register 20 (for the weight of both bodies).
-->
<option gravity="0 0 -1"/>
<default>
<!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
<geom size="0.05 0.05 0.05" type="box"/>
</default>
<worldbody>
<body name="body1">
<geom/>
<site name="sensor"/>
</body>
<body name="body2" pos="0 0 -0.1">
<joint type="slide" axis="0 0 1"/>
<geom/>
</body>
</worldbody>
<equality>
<weld body1="body1" body2="body2" anchor="0 0 0" tfratio="0"/>
</equality>
<sensor>
<force site="sensor" user="0 0 2"/>
</sensor>
</mujoco>
test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide_rotated.xml
Vendored
+43
View File
@@ -0,0 +1,43 @@
<!-- Copyright 2021 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco>
<!--
body2 is held by body1 through the weld constraint, so the force sensor on body1
should register 20 (for the weight of both bodies), rotated into the y-z plane.
-->
<option gravity="0 0 -1"/>
<default>
<!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
<geom size="0.05 0.05 0.05" type="box"/>
</default>
<worldbody>
<body name="body1" euler="0 45 0">
<geom/>
<site name="sensor" euler="0 0 90"/>
</body>
<body name="body2" pos="0 0 -0.2">
<joint type="slide" axis="0 0 1"/>
<geom/>
</body>
</worldbody>
<equality>
<weld body1="body1" body2="body2" anchor="0 0 0" tfratio="0"/>
</equality>
<sensor>
<force site="sensor" user="0 1.41421356237 1.41421356237"/>
</sensor>
</mujoco>
test/engine/testdata/core_smooth/rne_post/weld/tfratio0_multiple_constraints.xml
Vendored
+59
View File
@@ -0,0 +1,59 @@
<!-- Copyright 2021 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco>
<!--
Same as force_free.xml, but with a distractor constraint.
-->
<option gravity="1 2 3"/>
<default>
<!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
<geom size="0.05 0.05 0.05" type="box"/>
</default>
<worldbody>
<body name="body0" pos="-3 0 0">
<joint name="joint0" type="slide" axis="0 0 1"/>
<geom/>
</body>
<body name="body1">
<geom/>
<site name="sensor"/>
</body>
<body name="body2" pos="1 2 3">
<joint type="free"/>
<geom/>
</body>
<body name="body3" pos="3 0 0">
<geom/>
<body name="body3b" pos="3 0 -1">
<joint type="slide" axis="0 0 1" frictionloss="9"/>
<geom/>
</body>
</body>
<body name="body4" pos="4 0 0">
<joint type="free"/>
<geom/>
</body>
</worldbody>
<equality>
<joint joint1="joint0"/>
<weld body1="body3" body2="body4"/>
<weld body1="body1" body2="body2" anchor="0 0 0" tfratio="0"/>
</equality>
<sensor>
<force site="sensor" user="-2 -4 -6"/>
</sensor>
</mujoco>
test/engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml
Vendored
+43
View File
@@ -0,0 +1,43 @@
<!-- Copyright 2021 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco>
<!--
body2 is free, but because it's offset on y it wants to rotate about x due to gravity on z. The
connect prevents that, so the torque sensor should record the weight*offset of the second body.
-->
<option gravity="0 0 -1"/>
<default>
<!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
<geom size="0.05 0.05 0.05" type="box"/>
</default>
<worldbody>
<body name="body1">
<geom/>
<site name="sensor"/>
</body>
<body name="body2" pos="0 2 0">
<freejoint/>
<geom/>
</body>
</worldbody>
<equality>
<weld body1="body1" body2="body2" anchor="0 0 0" tfratio="0"/>
</equality>
<sensor>
<torque site="sensor" user="2 0 0"/>
</sensor>
</mujoco>
test/engine/testdata/weld.xml
Vendored
+58
View File
@@ -0,0 +1,58 @@
<mujoco>
<option>
<flag contact="disable"/>
</option>
<default>
<default class="static">
<geom size=".5 .1 .5" rgba=".5 .7 .5 .3"/>
</default>
<default class="free">
<geom type="box" size=".2" fromto="0 0 0 0 -2 0" rgba=".4 .7 .6 .3"/>
</default>
</default>
<worldbody>
<geom pos="0 0 -2" type="plane" size="10 10 .01"/>
<light pos="0 0 20"/>
<body name="box1" pos="-3 0 0">
<geom type="box" class="static"/>
</body>
<body name="beam1" pos="-3 0 0">
<freejoint/>
<geom class="free"/>
</body>
<body name="box2" pos="-1 0 0">
<geom type="box" class="static"/>
</body>
<body name="beam2" pos="-1 0 0">
<freejoint/>
<geom class="free"/>
</body>
<body name="box3" pos="1 0 0">
<geom type="box" class="static"/>
</body>
<body name="beam3" pos="1 0 0">
<freejoint/>
<geom class="free"/>
</body>
<body name="box4" pos="3 0 0">
<geom type="box" class="static"/>
</body>
<body name="beam4" pos="3 0 0">
<freejoint/>
<geom class="free"/>
</body>
</worldbody>
<equality>
<weld name="weak torques" body1="box1" body2="beam1" tfratio="0.002"/>
<weld name="anchor no torques" body1="box2" body2="beam2" tfratio="0" anchor="0 -2 0"/>
<weld name="relpose" body1="box3" body2="beam3" relpose="0 0 0 1 -.3 0 0"/>
<weld name="relpose+anchor" body1="box4" body2="beam4" relpose="0 0 0 1 -.3 0 0" anchor="0 0 -1"/>
</equality>
</mujoco>
unity/Runtime/Bindings/MujocoBindings.cs
+1 -1
View File
@@ -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;