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

Refactor a loop in mj_constraintUpdate.

Browse Source 
PiperOrigin-RevId: 561684763
Change-Id: Ifbca1793760ecb49f4b48a1a236bd7c1c6e73d0e
This commit is contained in:
Yuval Tassa
2023-08-31 09:58:41 -07:00
committed by Copybara-Service
parent eaa2572a8b
commit f9c57f6213
1 changed files with 42 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+42 -39
View File
@@ -2021,51 +2021,54 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
force[i] = -D[i]*jar[i];
}
// equality
for (int i=0; i < ne; i++) {
if (cost) {
s += 0.5*D[i]*jar[i]*jar[i];
}
d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
}
// friction
for (int i=ne; i < ne+nf; i++) {
// linear negative
if (jar[i] <= -R[i]*floss[i]) {
if (cost) {
s += -0.5*R[i]*floss[i]*floss[i] - floss[i]*jar[i];
}
force[i] = floss[i];
d->efc_state[i] = mjCNSTRSTATE_LINEARNEG;
}
// linear positive
else if (jar[i] >= R[i]*floss[i]) {
if (cost) {
s += -0.5*R[i]*floss[i]*floss[i] + floss[i]*jar[i];
}
force[i] = -floss[i];
d->efc_state[i] = mjCNSTRSTATE_LINEARPOS;
}
// quadratic
else {
// update constraints
for (int i=0; i < nefc; i++) {
// ==== equality
if (i < ne) {
if (cost) {
s += 0.5*D[i]*jar[i]*jar[i];
}
d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
continue;
}
}
// contact
for (int i=ne+nf; i < nefc; i++) {
// ==== friction
if (i < ne + nf) {
// linear negative
if (jar[i] <= -R[i]*floss[i]) {
if (cost) {
s += -0.5*R[i]*floss[i]*floss[i] - floss[i]*jar[i];
}
force[i] = floss[i];
d->efc_state[i] = mjCNSTRSTATE_LINEARNEG;
}
// linear positive
else if (jar[i] >= R[i]*floss[i]) {
if (cost) {
s += -0.5*R[i]*floss[i]*floss[i] + floss[i]*jar[i];
}
force[i] = -floss[i];
d->efc_state[i] = mjCNSTRSTATE_LINEARPOS;
}
// quadratic
else {
if (cost) {
s += 0.5*D[i]*jar[i]*jar[i];
}
d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
}
continue;
}
// ==== contact
// non-negative constraint
if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) {
// constraint is satisfied: no cost