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

Clean up engine_core_smooth.c

Browse Source 
PiperOrigin-RevId: 792564094
Change-Id: I47b3f1953db5402acc49dac373a039c9c0bf1ad9
This commit is contained in:
Yuval Tassa
2025-08-08 04:04:34 -07:00
committed by Copybara-Service
parent 92d77819fd
commit 3c6e4f1ece
1 changed files with 44 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+12 -5
View File
@@ -2125,10 +2125,11 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
mju_zero(loc_cfrc_body, 6); mju_zero(loc_cfrc_body, 6);
// backward pass over bodies: accumulate cfrc_body from children // backward pass over bodies: accumulate cfrc_body from children
for (int i=nbody-1; i > 0; i--) for (int i=nbody-1; i > 0; i--) {
if (m->body_parentid[i]) { if (m->body_parentid[i]) {
mju_addTo(loc_cfrc_body+6*m->body_parentid[i], loc_cfrc_body+6*i, 6); mju_addTo(loc_cfrc_body+6*m->body_parentid[i], loc_cfrc_body+6*i, 6);
} }
}
// result = cdof * cfrc_body // result = cdof * cfrc_body
for (int i=0; i < nv; i++) { for (int i=0; i < nv; i++) {
@@ -2154,7 +2155,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// cfrc_ext = perturb // cfrc_ext = perturb
mju_zero(d->cfrc_ext, 6*nbody); mju_zero(d->cfrc_ext, 6*nbody);
for (int i=1; i < nbody; i++) for (int i=1; i < nbody; i++) {
if (!mju_isZero(d->xfrc_applied+6*i, 6)) { if (!mju_isZero(d->xfrc_applied+6*i, 6)) {
// rearrange as torque:force // rearrange as torque:force
mju_copy3(cfrc, d->xfrc_applied+6*i+3); mju_copy3(cfrc, d->xfrc_applied+6*i+3);
@@ -2166,14 +2167,19 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// accumulate // accumulate
mju_addTo(d->cfrc_ext+6*i, cfrc_com, 6); mju_addTo(d->cfrc_ext+6*i, cfrc_com, 6);
} }
}
// cfrc_ext += contacts // cfrc_ext += contacts
int ncon = d->ncon; int ncon = d->ncon;
for (int i=0; i < ncon; i++) for (int i=0; i < ncon; i++) {
if (d->contact[i].efc_address >= 0) {
// get contact pointer // get contact pointer
con = d->contact+i; con = d->contact+i;
// skip excluded contacts
if (con->efc_address < 0) {
continue;
}
// skip contact involving flex // skip contact involving flex
if (con->geom[0] < 0 || con->geom[1] < 0) { if (con->geom[0] < 0 || con->geom[1] < 0) {
continue; continue;
@@ -2209,8 +2215,9 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// cfrc_ext += connect, weld, flex constraints // cfrc_ext += connect, weld, flex constraints
int i = 0, ne = d->ne; int i = 0, ne = d->ne;
while (i < ne) { while (i < ne) {
if (d->efc_type[i] != mjCNSTR_EQUALITY) if (d->efc_type[i] != mjCNSTR_EQUALITY) {
mjERROR("row %d of efc is not an equality constraint", i); // SHOULD NOT OCCUR mjERROR("row %d of efc is not an equality constraint", i); // SHOULD NOT OCCUR
}
int id = d->efc_id[i]; int id = d->efc_id[i];
mjtNum* eq_data = m->eq_data + mjNEQDATA*id; mjtNum* eq_data = m->eq_data + mjNEQDATA*id;