chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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: 469228149
Change-Id: I836b0791f10fb624607a12ef3c687da991c21789
This commit is contained in:
Alessio Quaglino
2022-08-22 10:46:12 -07:00
committed by Copybara-Service
parent 558aaf2923
commit abc0a39b7a
20 changed files with 250 additions and 492 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+20 -25
View File
@@ -398,16 +398,18 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
break;
case mjEQ_WELD: // fix relative position and orientation
// find global points
// find global points and their Jacobians
for (int j=0; j<2; j++) {
mjtNum* anchor = data + 3*(1-j);
mju_rotVecMat(pos[j], anchor, d->xmat + 9*id[j]);
// position offset for body1 only
if (j==0) {
mju_rotVecMat(pos[j], data, d->xmat + 9*id[j]);
} else {
mju_zero3(pos[j]);
}
mju_addTo3(pos[j], d->xpos + 3*id[j]);
}
// compute position error
mju_sub3(cpos, pos[0], pos[1]);
// compute error Jacobian (opposite of contact: 0 - 1)
NV = mj_jacDifPair(m, d, chain, id[1], id[0], pos[1], pos[0],
jac[1], jac[0], jacdif,
@@ -417,14 +419,19 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
mju_copy(jac[0], jacdif, 3*NV);
mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV);
// compute orientation error: neg(q1) * q0 * relpose (axis components only)
mjtNum* relpose = data+6;
mju_mulQuat(quat, d->xquat+4*id[0], relpose); // quat = q0*relpose
// get desired position offset in global frame
mju_rotVecMat(cpos, data, d->xmat+9*id[0]);
// compute position error: p0 - p1 - data
mju_sub3(cpos, pos[0], pos[1]);
// compute orientation error: neg(q1) * q0 * data (axis components only)
mju_mulQuat(quat, d->xquat+4*id[0], data+3); // quat = q0*data
mju_negQuat(quat1, d->xquat+4*id[1]); // quat1 = neg(q1)
mju_mulQuat(quat2, quat1, quat); // quat2 = neg(q1)*q0*relpose
mju_mulQuat(quat2, quat1, quat); // quat2 = neg(q1)*q0*data
mju_copy3(cpos+3, quat2+1); // copy axis components
// correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose
// correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * data
for (int j=0; j<NV; j++) {
// axis = [jac0-jac1]_col(j)
axis[0] = jac[0][3*NV+j];
@@ -433,7 +440,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*relpose
mju_mulQuat(quat3, quat2, quat); // quat3 = neg(q1)*(jac0-jac1)*q0*data
// correct Jacobian
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];
}
// 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;
@@ -1051,16 +1054,8 @@ 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(efc_pos, 6);
*pos = mju_norm(d->efc_pos+i, 6); // mixes translation and rotation!
} else if (m->eq_type[id]==mjEQ_CONNECT) {
*dim = 3;
*pos = mju_norm(d->efc_pos+i, 3);