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

Only apply torquescale once, when setting efc_pos.

Browse Source 
PiperOrigin-RevId: 580694671
Change-Id: Ic0e2ee773d65d4fd92f4b4102cdfdd040516680a
This commit is contained in:
Erik Frey
2023-11-08 16:14:53 -08:00
committed by Copybara-Service
parent cdf55e2ae9
commit d0cabcf7db
1 changed files with 1 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+1 -9
View File
@@ -1310,16 +1310,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 torquescale
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);
} else if (m->eq_type[id] == mjEQ_CONNECT) {
*dim = 3;
*pos = mju_norm(d->efc_pos+i, 3);