chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 10 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_setconst.c
+10 -13
View File
@@ -185,25 +185,22 @@ static void set0(mjModel* m, mjData* d) {
// weld constraint
else if (m->eq_type[i]==mjEQ_WELD) {
// skip if user has set any quaternion data
if (m->eq_data[mjNEQDATA*i+6] ||
m->eq_data[mjNEQDATA*i+7] ||
m->eq_data[mjNEQDATA*i+8] ||
m->eq_data[mjNEQDATA*i+9]) {
if (m->eq_data[mjNEQDATA*i+3] ||
m->eq_data[mjNEQDATA*i+4] ||
m->eq_data[mjNEQDATA*i+5] ||
m->eq_data[mjNEQDATA*i+6]) {
// normalize quaternion just in case
mju_normalize4(m->eq_data+mjNEQDATA*i+6);
mju_normalize4(m->eq_data+mjNEQDATA*i+3);
continue;
}
// anchor position is in body2 local frame
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0);
// data[0-2] = xpos2-xpos1 in body1 local frame
mju_sub3(pos, d->xpos+3*id2, d->xpos+3*id1);
mju_rotVecMatT(m->eq_data+mjNEQDATA*i, pos, d->xmat+9*id1);
// data[3-5] = anchor position in body1 local frame
mju_subFrom3(pos, d->xpos+3*id1);
mju_rotVecMatT(m->eq_data+mjNEQDATA*i+3, pos, d->xmat+9*id1);
// data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
// data[3-6] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
mju_negQuat(quat, d->xquat+4*id1);
mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2);
mju_mulQuat(m->eq_data+mjNEQDATA*i+3, quat, d->xquat+4*id2);
}
}