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
This commit is contained in:
committed by
Copybara-Service
parent
558aaf2923
commit
abc0a39b7a
@@ -3162,7 +3162,7 @@ Numeric constants
|
|||||||
+------------------+--------+----------------------------------------------------------------------------------------+
|
+------------------+--------+----------------------------------------------------------------------------------------+
|
||||||
| mjMAXVFSNAME | 100 | The maximal number of characters in the name of each file in the virtual file system. |
|
| 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 |
|
| | | 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 |
|
| | | 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 |
|
| | | the future, so as to accommodate extra parameters needed for more elaborate |
|
||||||
|
|||||||
+1
-13
@@ -812,9 +812,7 @@ in the second column of the table have the following meaning:
|
|||||||
| | | +-------------------------+-------------------------+-------------------------+ |
|
| | | +-------------------------+-------------------------+-------------------------+ |
|
||||||
| | | | :at:`body2` | :at:`relpose` | :at:`active` | |
|
| | | | :at:`body2` | :at:`relpose` | :at:`active` | |
|
||||||
| | | +-------------------------+-------------------------+-------------------------+ |
|
| | | +-------------------------+-------------------------+-------------------------+ |
|
||||||
| | | | :at:`solref` | :at:`solimp` | :at:`anchor` | |
|
| | | | :at:`solref` | :at:`solimp` | | |
|
||||||
| | | +-------------------------+-------------------------+-------------------------+ |
|
|
||||||
| | | | :at:`tfratio` | | | |
|
|
||||||
| | | +-------------------------+-------------------------+-------------------------+ |
|
| | | +-------------------------+-------------------------+-------------------------+ |
|
||||||
+--------------------------+----+------------------------------------------------------------------------------------+
|
+--------------------------+----+------------------------------------------------------------------------------------+
|
||||||
| |_2|:el:`joint` | \* | .. table:: |
|
| |_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
|
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
|
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.
|
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:
|
.. _equality-joint:
|
||||||
|
|
||||||
|
|||||||
+2
-9
@@ -15,13 +15,8 @@ General
|
|||||||
- Added :ref:`adhesion actuators<adhesion>` mimicking vacuum grippers and adhesive biomechanical appendages.
|
- 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 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 :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
|
- Added moment of inertia computation for concave meshes. This is currently activated by setting the compiler flag
|
||||||
ratio exerted by the constraint, :at:`anchor` sets the point at which the weld wrench is applied. See
|
:at:`exactmeshinertia` to ``true`` (defaults to ``false``). This default may change in the future.
|
||||||
: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
|
- 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
|
now default to ``auto`` rather than ``false``. Limits are automatically set to ``true`` if the corresponding range *is
|
||||||
defined* and ``false`` otherwise.
|
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
|
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.
|
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).
|
- Added parameter :at:`shellinertia` to :at:`geom`, for locating the inferred inertia on the boundary (shell).
|
||||||
Currently only meshes are supported.
|
Currently only meshes are supported.
|
||||||
- For meshes from which volumetric inertia is inferred, raise error if the orientation of mesh faces is not consistent.
|
- For meshes from which volumetric inertia is inferred, raise error if the orientation of mesh faces is not consistent.
|
||||||
|
|||||||
@@ -30,7 +30,7 @@
|
|||||||
|
|
||||||
//---------------------------------- sizes ---------------------------------------------------------
|
//---------------------------------- 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 mjNDYN 10 // number of actuator dynamics parameters
|
||||||
#define mjNGAIN 10 // number of actuator gain parameters
|
#define mjNGAIN 10 // number of actuator gain parameters
|
||||||
#define mjNBIAS 10 // number of actuator bias parameters
|
#define mjNBIAS 10 // number of actuator bias parameters
|
||||||
|
|||||||
@@ -398,16 +398,18 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case mjEQ_WELD: // fix relative position and orientation
|
case mjEQ_WELD: // fix relative position and orientation
|
||||||
// find global points
|
// find global points and their Jacobians
|
||||||
for (int j=0; j<2; j++) {
|
for (int j=0; j<2; j++) {
|
||||||
mjtNum* anchor = data + 3*(1-j);
|
// position offset for body1 only
|
||||||
mju_rotVecMat(pos[j], anchor, d->xmat + 9*id[j]);
|
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]);
|
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)
|
// compute error Jacobian (opposite of contact: 0 - 1)
|
||||||
NV = mj_jacDifPair(m, d, chain, id[1], id[0], pos[1], pos[0],
|
NV = mj_jacDifPair(m, d, chain, id[1], id[0], pos[1], pos[0],
|
||||||
jac[1], jac[0], jacdif,
|
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], jacdif, 3*NV);
|
||||||
mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV);
|
mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV);
|
||||||
|
|
||||||
// compute orientation error: neg(q1) * q0 * relpose (axis components only)
|
// get desired position offset in global frame
|
||||||
mjtNum* relpose = data+6;
|
mju_rotVecMat(cpos, data, d->xmat+9*id[0]);
|
||||||
mju_mulQuat(quat, d->xquat+4*id[0], relpose); // quat = q0*relpose
|
|
||||||
|
// 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_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
|
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; j<NV; j++) {
|
for (int j=0; j<NV; j++) {
|
||||||
// axis = [jac0-jac1]_col(j)
|
// axis = [jac0-jac1]_col(j)
|
||||||
axis[0] = jac[0][3*NV+j];
|
axis[0] = jac[0][3*NV+j];
|
||||||
@@ -433,7 +440,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
|||||||
|
|
||||||
// apply formula
|
// apply formula
|
||||||
mju_mulQuatAxis(quat2, quat1, axis); // quat2 = neg(q1)*(jac0-jac1)
|
mju_mulQuatAxis(quat2, quat1, axis); // quat2 = neg(q1)*(jac0-jac1)
|
||||||
mju_mulQuat(quat3, quat2, quat); // quat3 = neg(q1)*(jac0-jac1)*q0*relpose
|
mju_mulQuat(quat3, quat2, quat); // quat3 = neg(q1)*(jac0-jac1)*q0*data
|
||||||
|
|
||||||
// correct Jacobian
|
// correct Jacobian
|
||||||
jac[0][3*NV+j] = 0.5*quat3[1];
|
jac[0][3*NV+j] = 0.5*quat3[1];
|
||||||
@@ -441,10 +448,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
|||||||
jac[0][5*NV+j] = 0.5*quat3[3];
|
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;
|
size = 6;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
@@ -1051,16 +1054,8 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int
|
|||||||
|
|
||||||
case mjCNSTR_EQUALITY:
|
case mjCNSTR_EQUALITY:
|
||||||
if (m->eq_type[id]==mjEQ_WELD) {
|
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;
|
*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) {
|
} else if (m->eq_type[id]==mjEQ_CONNECT) {
|
||||||
*dim = 3;
|
*dim = 3;
|
||||||
*pos = mju_norm(d->efc_pos+i, 3);
|
*pos = mju_norm(d->efc_pos+i, 3);
|
||||||
|
|||||||
@@ -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
|
// RNE with complete data: compute cacc, cfrc_ext, cfrc_int
|
||||||
void mj_rnePostConstraint(const mjModel* m, mjData* d) {
|
void mj_rnePostConstraint(const mjModel* m, mjData* d) {
|
||||||
int nbody=m->nbody;
|
int nbody=m->nbody;
|
||||||
mjtNum cfrc_com[6], cfrc[6], lfrc[6];
|
mjtNum cfrc_body[6], tmp[6], tmp1[6];
|
||||||
mjContact* con;
|
mjContact* con;
|
||||||
|
|
||||||
// clear cacc, set world acceleration to -gravity
|
// 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++)
|
for (int i=1; i<nbody; i++)
|
||||||
if (!mju_isZero(d->xfrc_applied+6*i, 6)) {
|
if (!mju_isZero(d->xfrc_applied+6*i, 6)) {
|
||||||
// rearrange as torque:force
|
// rearrange as torque:force
|
||||||
mju_copy3(cfrc, d->xfrc_applied+6*i+3);
|
mju_copy3(tmp1, d->xfrc_applied+6*i+3);
|
||||||
mju_copy3(cfrc+3, d->xfrc_applied+6*i);
|
mju_copy3(tmp1+3, d->xfrc_applied+6*i);
|
||||||
|
|
||||||
// map force from application point to com; both world-oriented
|
// 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
|
// accumulate
|
||||||
mju_addTo(d->cfrc_ext+6*i, cfrc_com, 6);
|
mju_addTo(d->cfrc_ext+6*i, tmp, 6);
|
||||||
}
|
}
|
||||||
|
|
||||||
// cfrc_ext += contacts
|
// cfrc_ext += contacts
|
||||||
@@ -1845,29 +1845,29 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
|
|||||||
con = d->contact+i;
|
con = d->contact+i;
|
||||||
|
|
||||||
// tmp = contact-local force:torque vector
|
// 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)
|
// tmp1 = world-oriented torque:force vector (swap in the process)
|
||||||
mju_rotVecMatT(cfrc, lfrc+3, con->frame);
|
mju_rotVecMatT(tmp1, tmp+3, con->frame);
|
||||||
mju_rotVecMatT(cfrc+3, lfrc, con->frame);
|
mju_rotVecMatT(tmp1+3, tmp, con->frame);
|
||||||
|
|
||||||
// body 1
|
// body 1
|
||||||
int k;
|
int k;
|
||||||
if ((k = m->geom_bodyid[con->geom1])) {
|
if ((k = m->geom_bodyid[con->geom1])) {
|
||||||
// tmp = subtree CoM-based torque_force vector
|
// 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)
|
// 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
|
// body 2
|
||||||
if ((k = m->geom_bodyid[con->geom2])) {
|
if ((k = m->geom_bodyid[con->geom2])) {
|
||||||
// tmp = subtree CoM-based torque_force vector
|
// 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
|
// 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;
|
int k;
|
||||||
switch (m->eq_type[id]) {
|
switch (m->eq_type[id]) {
|
||||||
case mjEQ_CONNECT:
|
case mjEQ_CONNECT:
|
||||||
case mjEQ_WELD:
|
// tmp1 = world-oriented torque:force vector
|
||||||
// cfrc = world-oriented torque:force vector
|
mju_zero3(tmp1); // no torque from connect
|
||||||
mju_copy3(cfrc + 3, d->efc_force + i);
|
mju_copy3(tmp1 + 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
|
// body 1
|
||||||
if ((k = m->eq_obj1id[id])) {
|
if ((k = m->eq_obj1id[id])) {
|
||||||
// transform point on body1: local -> global
|
// transform connect point on body1: local -> global
|
||||||
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_WELD), 0, k, 0);
|
mju_rotVecMat(pos, eq_data, d->xmat+9*k);
|
||||||
|
mju_addTo3(pos, d->xpos+3*k);
|
||||||
|
|
||||||
// tmp = subtree CoM-based torque_force vector
|
// 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)
|
// 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
|
// body 2
|
||||||
if ((k = m->eq_obj2id[id])) {
|
if ((k = m->eq_obj2id[id])) {
|
||||||
// transform point on body2: local -> global
|
// transform connect point on body2: local -> global
|
||||||
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_CONNECT), 0, k, 0);
|
mju_rotVecMat(pos, eq_data + 3, d->xmat+9*k);
|
||||||
|
mju_addTo3(pos, d->xpos+3*k);
|
||||||
|
|
||||||
// tmp = subtree CoM-based torque_force vector
|
// 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
|
// apply
|
||||||
mju_subFrom(d->cfrc_ext+6*k, cfrc_com, 6);
|
mju_subFrom(d->cfrc_ext+6*k, tmp, 6);
|
||||||
}
|
}
|
||||||
|
|
||||||
// increment rows
|
// increment 3 rows of connect
|
||||||
i += m->eq_type[id]==mjEQ_WELD ? 6 : 3;
|
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;
|
break;
|
||||||
|
|
||||||
case mjEQ_JOINT:
|
case mjEQ_JOINT:
|
||||||
@@ -1935,23 +1967,22 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// forward pass over bodies: compute cacc, cfrc_int
|
// forward pass over bodies: compute cacc, cfrc_int
|
||||||
mjtNum cacc[6], cfrc_body[6], cfrc_corr[6];
|
|
||||||
mju_zero(d->cfrc_int, 6);
|
mju_zero(d->cfrc_int, 6);
|
||||||
for (int i=1; i<m->nbody; i++) {
|
for (int i=1; i<m->nbody; i++) {
|
||||||
// get body's first dof address
|
// get body's first dof address
|
||||||
int bda = m->body_dofadr[i];
|
int bda = m->body_dofadr[i];
|
||||||
|
|
||||||
// cacc = cacc_parent + cdofdot * qvel + cdof * qacc
|
// cacc = cacc_parent + cdofdot * qvel + cdof * qacc
|
||||||
mju_mulDofVec(cacc, d->cdof_dot+6*bda, d->qvel+bda, m->body_dofnum[i]);
|
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], cacc, 6);
|
mju_add(d->cacc+6*i, d->cacc+6*m->body_parentid[i], tmp, 6);
|
||||||
mju_mulDofVec(cacc, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[i]);
|
mju_mulDofVec(tmp, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[i]);
|
||||||
mju_addTo(d->cacc+6*i, cacc, 6);
|
mju_addTo(d->cacc+6*i, tmp, 6);
|
||||||
|
|
||||||
// cfrc_body = cinert * cacc + cvel x (cinert * cvel)
|
// cfrc_body = cinert * cacc + cvel x (cinert * cvel)
|
||||||
mju_mulInertVec(cfrc_body, d->cinert+10*i, d->cacc+6*i);
|
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_mulInertVec(tmp, d->cinert+10*i, d->cvel+6*i);
|
||||||
mju_crossForce(cfrc, d->cvel+6*i, cfrc_corr);
|
mju_crossForce(tmp1, d->cvel+6*i, tmp);
|
||||||
mju_addTo(cfrc_body, cfrc, 6);
|
mju_addTo(cfrc_body, tmp1, 6);
|
||||||
|
|
||||||
// set cfrc_int = cfrc_body - cfrc_ext
|
// set cfrc_int = cfrc_body - cfrc_ext
|
||||||
mju_sub(d->cfrc_int+6*i, cfrc_body, d->cfrc_ext+6*i, 6);
|
mju_sub(d->cfrc_int+6*i, cfrc_body, d->cfrc_ext+6*i, 6);
|
||||||
|
|||||||
@@ -185,25 +185,22 @@ static void set0(mjModel* m, mjData* d) {
|
|||||||
// weld constraint
|
// weld constraint
|
||||||
else if (m->eq_type[i]==mjEQ_WELD) {
|
else if (m->eq_type[i]==mjEQ_WELD) {
|
||||||
// skip if user has set any quaternion data
|
// skip if user has set any quaternion data
|
||||||
if (m->eq_data[mjNEQDATA*i+6] ||
|
if (m->eq_data[mjNEQDATA*i+3] ||
|
||||||
m->eq_data[mjNEQDATA*i+7] ||
|
m->eq_data[mjNEQDATA*i+4] ||
|
||||||
m->eq_data[mjNEQDATA*i+8] ||
|
m->eq_data[mjNEQDATA*i+5] ||
|
||||||
m->eq_data[mjNEQDATA*i+9]) {
|
m->eq_data[mjNEQDATA*i+6]) {
|
||||||
// normalize quaternion just in case
|
// normalize quaternion just in case
|
||||||
mju_normalize4(m->eq_data+mjNEQDATA*i+6);
|
mju_normalize4(m->eq_data+mjNEQDATA*i+3);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
// anchor position is in body2 local frame
|
// data[0-2] = xpos2-xpos1 in body1 local frame
|
||||||
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0);
|
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
|
// data[3-6] = neg(xquat1)*xquat2 = "xquat2-xquat1" 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_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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1606,31 +1606,33 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
|||||||
objtype = mjOBJ_EQUALITY;
|
objtype = mjOBJ_EQUALITY;
|
||||||
category = mjCAT_DECOR;
|
category = mjCAT_DECOR;
|
||||||
if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) {
|
if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) {
|
||||||
// connect or weld
|
// connect
|
||||||
for (int i=0; i<m->neq; i++) {
|
for (int i=0; i<m->neq; 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
|
// compute endpoints in global coordinates
|
||||||
int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
|
int j = m->eq_obj1id[i];
|
||||||
mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i]==mjEQ_WELD), d->xmat+9*j);
|
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_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);
|
mju_addTo3(end, d->xpos+3*k);
|
||||||
|
|
||||||
|
// connect endpoints
|
||||||
|
START
|
||||||
|
|
||||||
// construct geom
|
// construct geom
|
||||||
sz[0] = scl * m->vis.scale.constraint;
|
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
|
f2f(thisgeom->rgba, m->vis.rgba.constraint, 4);
|
||||||
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, d->xmat+9*j, m->vis.rgba.connect);
|
|
||||||
|
// label flag
|
||||||
if (vopt->label==mjLABEL_CONSTRAINT) {
|
if (vopt->label==mjLABEL_CONSTRAINT) {
|
||||||
makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);
|
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
|
FINISH
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -2884,7 +2884,6 @@ mjCEquality::mjCEquality(mjCModel* _model, mjCDef* _def) {
|
|||||||
|
|
||||||
mjuu_zerovec(data, mjNEQDATA);
|
mjuu_zerovec(data, mjNEQDATA);
|
||||||
data[1] = 1;
|
data[1] = 1;
|
||||||
data[10] = 1; // torque:force ratio
|
|
||||||
|
|
||||||
// clear internal variables
|
// clear internal variables
|
||||||
obj1id = obj2id = -1;
|
obj1id = obj2id = -1;
|
||||||
@@ -2972,11 +2971,6 @@ void mjCEquality::Compile(void) {
|
|||||||
if (type==mjEQ_CONNECT) {
|
if (type==mjEQ_CONNECT) {
|
||||||
((mjCBody*)px1)->MakeLocal(anchor, qdummy, data, qunit);
|
((mjCBody*)px1)->MakeLocal(anchor, qdummy, data, qunit);
|
||||||
mjuu_copyvec(data, anchor, 3);
|
mjuu_copyvec(data, anchor, 3);
|
||||||
} else if (type==mjEQ_WELD) {
|
|
||||||
if (px2) {
|
|
||||||
((mjCBody*)px2)->MakeLocal(anchor, qdummy, data, qunit);
|
|
||||||
mjuu_copyvec(data, anchor, 3);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -752,8 +752,8 @@ class mjCEquality : public mjCBase {
|
|||||||
double data[mjNEQDATA]; // type-dependent data
|
double data[mjNEQDATA]; // type-dependent data
|
||||||
|
|
||||||
private:
|
private:
|
||||||
mjCEquality(mjCModel* = 0, mjCDef* = 0); // constructor
|
mjCEquality(mjCModel* = 0, mjCDef* = 0);// constructor
|
||||||
void Compile(void); // compiler
|
void Compile(void); // compiler
|
||||||
|
|
||||||
int obj1id; // id of object 1
|
int obj1id; // id of object 1
|
||||||
int obj2id; // id of object 2
|
int obj2id; // id of object 2
|
||||||
|
|||||||
@@ -234,8 +234,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
|
|||||||
{"<"},
|
{"<"},
|
||||||
{"connect", "*", "8", "name", "class", "body1", "body2", "anchor",
|
{"connect", "*", "8", "name", "class", "body1", "body2", "anchor",
|
||||||
"active", "solref", "solimp"},
|
"active", "solref", "solimp"},
|
||||||
{"weld", "*", "10", "name", "class", "body1", "body2", "relpose", "anchor",
|
{"weld", "*", "8", "name", "class", "body1", "body2", "relpose",
|
||||||
"active", "solref", "solimp", "tfratio"},
|
"active", "solref", "solimp"},
|
||||||
{"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef",
|
{"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef",
|
||||||
"active", "solref", "solimp"},
|
"active", "solref", "solimp"},
|
||||||
{"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef",
|
{"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef",
|
||||||
@@ -1310,11 +1310,7 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
|
|||||||
case mjEQ_WELD:
|
case mjEQ_WELD:
|
||||||
ReadAttrTxt(elem, "body1", pequality->name1, true);
|
ReadAttrTxt(elem, "body1", pequality->name1, true);
|
||||||
ReadAttrTxt(elem, "body2", pequality->name2);
|
ReadAttrTxt(elem, "body2", pequality->name2);
|
||||||
ReadAttr(elem, "relpose", 7, pequality->data+3, text);
|
ReadAttr(elem, "relpose", mjNEQDATA, pequality->data, text);
|
||||||
ReadAttr(elem, "tfratio", 1, pequality->data+10, text);
|
|
||||||
if (!ReadAttr(elem, "anchor", 3, pequality->data, text)) {
|
|
||||||
mjuu_zerovec(pequality->data, 3);
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case mjEQ_JOINT:
|
case mjEQ_JOINT:
|
||||||
|
|||||||
@@ -456,9 +456,7 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
|
|||||||
case mjEQ_WELD:
|
case mjEQ_WELD:
|
||||||
WriteAttrTxt(elem, "body1", peq->name1);
|
WriteAttrTxt(elem, "body1", peq->name1);
|
||||||
WriteAttrTxt(elem, "body2", peq->name2);
|
WriteAttrTxt(elem, "body2", peq->name2);
|
||||||
WriteAttr(elem, "anchor", 3, peq->data);
|
WriteAttr(elem, "relpose", 7, peq->data);
|
||||||
WriteAttr(elem, "tfratio", 1, peq->data+10);
|
|
||||||
WriteAttr(elem, "relpose", 7, peq->data+3);
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case mjEQ_JOINT:
|
case mjEQ_JOINT:
|
||||||
|
|||||||
@@ -56,9 +56,10 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) {
|
|||||||
|
|
||||||
// --------------------------- connect constraint ------------------------------
|
// --------------------------- connect constraint ------------------------------
|
||||||
|
|
||||||
// test that bodies hanging on connects lead to expected force sensor readings
|
TEST_F(CoreSmoothTest, RnePostConnectForceSlide) {
|
||||||
void TestConnect(const char* const filepath) {
|
static const char* const kModelFilePath =
|
||||||
const std::string xml_path = GetTestDataFilePath(filepath);
|
"engine/testdata/core_smooth/rne_post/connect/force_slide.xml";
|
||||||
|
const std::string xml_path = GetTestDataFilePath(kModelFilePath);
|
||||||
mjModel* model =
|
mjModel* model =
|
||||||
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
||||||
mjData* data = mj_makeData(model);
|
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) {
|
TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) {
|
||||||
constexpr char kModelFilePath[] =
|
static const char* const kModelFilePath =
|
||||||
"engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml";
|
"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) {
|
TEST_F(CoreSmoothTest, RnePostConnectForceFree) {
|
||||||
constexpr char kModelFilePath[] =
|
static const char* const kModelFilePath =
|
||||||
"engine/testdata/core_smooth/rne_post/connect/force_free.xml";
|
"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) {
|
TEST_F(CoreSmoothTest, RnePostConnectTorque) {
|
||||||
constexpr char kModelFilePath[] =
|
static const char* const kModelFilePath =
|
||||||
"engine/testdata/core_smooth/rne_post/connect/torque_free.xml";
|
"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) {
|
TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) {
|
||||||
constexpr char kModelFilePath[] =
|
static const char* const kModelFilePath =
|
||||||
"engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml";
|
"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 ---------------------------------
|
// --------------------------- weld constraint ---------------------------------
|
||||||
|
|
||||||
// test that bodies attached with welds lead to expected force sensor readings
|
TEST_F(CoreSmoothTest, RnePostWeldForceFree) {
|
||||||
void TestWeld(const char* const filepath) {
|
static const char* const kModelFilePath =
|
||||||
const std::string xml_path = GetTestDataFilePath(filepath);
|
"engine/testdata/core_smooth/rne_post/weld/force_free.xml";
|
||||||
|
const std::string xml_path = GetTestDataFilePath(kModelFilePath);
|
||||||
mjModel* model =
|
mjModel* model =
|
||||||
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
|
||||||
mjData* data = mj_makeData(model);
|
mjData* data = mj_makeData(model);
|
||||||
@@ -134,65 +177,75 @@ void TestWeld(const char* const filepath) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, RnePostWeldForceFree) {
|
TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotatoed) {
|
||||||
constexpr char kModelFilePath[] =
|
static const char* const 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";
|
"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) {
|
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) {
|
||||||
constexpr char kModelFilePath[] =
|
static const char* const kModelFilePath =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml";
|
"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) {
|
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) {
|
||||||
constexpr char kModelFilePath[] =
|
static const char* const kModelFilePath =
|
||||||
"engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml";
|
"engine/testdata/core_smooth/rne_post/weld/force_torque_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);
|
||||||
TEST_F(CoreSmoothTest, WeldRatioForceFree) {
|
// settle physics:
|
||||||
constexpr char kModelFilePath[] =
|
for (int i=0; i < 1000; i++) {
|
||||||
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml";
|
mj_step(model, data);
|
||||||
TestConnect(kModelFilePath);
|
}
|
||||||
}
|
for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) {
|
||||||
|
for (int i=0; i < 3; i++) {
|
||||||
|
EXPECT_NEAR(
|
||||||
TEST_F(CoreSmoothTest, WeldRatioForceSlide) {
|
data->sensordata[model->sensor_adr[sensor_index] + i],
|
||||||
constexpr char kModelFilePath[] =
|
model->sensor_user[model->nuser_sensor*sensor_index + i],
|
||||||
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml";
|
1e-6);
|
||||||
TestConnect(kModelFilePath);
|
}
|
||||||
}
|
}
|
||||||
|
mj_deleteData(data);
|
||||||
|
mj_deleteModel(model);
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -1,43 +0,0 @@
|
|||||||
<!-- 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>
|
|
||||||
@@ -1,43 +0,0 @@
|
|||||||
<!-- 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>
|
|
||||||
-43
@@ -1,43 +0,0 @@
|
|||||||
<!-- 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>
|
|
||||||
-59
@@ -1,59 +0,0 @@
|
|||||||
<!-- 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>
|
|
||||||
@@ -1,43 +0,0 @@
|
|||||||
<!-- 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>
|
|
||||||
Vendored
-58
@@ -1,58 +0,0 @@
|
|||||||
<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>
|
|
||||||
@@ -40,7 +40,7 @@ public const double mjMAXIMP = 0.9999;
|
|||||||
public const int mjMAXCONPAIR = 50;
|
public const int mjMAXCONPAIR = 50;
|
||||||
public const int mjMAXVFS = 2000;
|
public const int mjMAXVFS = 2000;
|
||||||
public const int mjMAXVFSNAME = 1000;
|
public const int mjMAXVFSNAME = 1000;
|
||||||
public const int mjNEQDATA = 11;
|
public const int mjNEQDATA = 7;
|
||||||
public const int mjNDYN = 10;
|
public const int mjNDYN = 10;
|
||||||
public const int mjNGAIN = 10;
|
public const int mjNGAIN = 10;
|
||||||
public const int mjNBIAS = 10;
|
public const int mjNBIAS = 10;
|
||||||
|
|||||||
Reference in New Issue
Block a user