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
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);
}
}