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
+10 -7
View File
@@ -2021,17 +2021,19 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
force[i] = -D[i]*jar[i]; force[i] = -D[i]*jar[i];
} }
// equality // update constraints
for (int i=0; i < ne; i++) { for (int i=0; i < nefc; i++) {
// ==== equality
if (i < ne) {
if (cost) { if (cost) {
s += 0.5*D[i]*jar[i]*jar[i]; s += 0.5*D[i]*jar[i]*jar[i];
} }
d->efc_state[i] = mjCNSTRSTATE_QUADRATIC; d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
continue;
} }
// friction // ==== friction
for (int i=ne; i < ne+nf; i++) { if (i < ne + nf) {
// linear negative // linear negative
if (jar[i] <= -R[i]*floss[i]) { if (jar[i] <= -R[i]*floss[i]) {
if (cost) { if (cost) {
@@ -2062,10 +2064,11 @@ void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar,
d->efc_state[i] = mjCNSTRSTATE_QUADRATIC; d->efc_state[i] = mjCNSTRSTATE_QUADRATIC;
} }
continue;
} }
// contact // ==== contact
for (int i=ne+nf; i < nefc; i++) {
// non-negative constraint // non-negative constraint
if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) { if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) {
// constraint is satisfied: no cost // constraint is satisfied: no cost