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

Fix multiple bugs related to connect and weld constraints with site semantics. Fixes #2179

Browse Source 
The introduction of site specification to connects and welds in 3.2.3 conditionally changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`. These changes were not properly propagated in several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and runtime enabling/disabling of such constraints.

PiperOrigin-RevId: 690670420
Change-Id: I55ee8a013cbee8457f8d6c7f33c2981aedafbab6
This commit is contained in:
Yuval Tassa
2024-10-28 10:43:09 -07:00
committed by Copybara-Service
parent 2c86136dd6
commit 864b805a6e
10 changed files with 234 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+19 -7
View File
@@ -1902,7 +1902,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
}
// cfrc_ext += connect and weld constraints
// cfrc_ext += connect, weld, flex constraints
int i = 0, ne = d->ne;
while (i < ne) {
if (d->efc_type[i] != mjCNSTR_EQUALITY)
@@ -1910,8 +1910,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
int id = d->efc_id[i];
mjtNum* eq_data = m->eq_data + mjNEQDATA*id;
mjtNum pos[3];
int k;
mjtNum pos[3], *offset;
int k, obj1, obj2, body_semantic;
switch ((mjtEq) m->eq_type[id]) {
case mjEQ_CONNECT:
case mjEQ_WELD:
@@ -1923,10 +1923,17 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
mju_zero3(cfrc); // no torque from connect
}
body_semantic = m->eq_objtype[id] == mjOBJ_BODY;
// body 1
if ((k = m->eq_obj1id[id])) {
obj1 = m->eq_obj1id[id];
k = body_semantic ? obj1 : m->site_bodyid[obj1];
if (k) {
offset = body_semantic ? eq_data + 3 * (m->eq_type[id] == mjEQ_WELD) :
m->site_pos + 3 * obj1;
// transform point on body1: local -> global
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_WELD), 0, k, 0);
mj_local2Global(d, pos, 0, offset, 0, k, 0);
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);
@@ -1936,9 +1943,14 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
// body 2
if ((k = m->eq_obj2id[id])) {
obj2 = m->eq_obj2id[id];
k = body_semantic ? obj2 : m->site_bodyid[obj2];
if (k) {
offset = body_semantic ? eq_data + 3 * (m->eq_type[id] == mjEQ_CONNECT) :
m->site_pos + 3 * obj2;
// transform point on body2: local -> global
mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id] == mjEQ_CONNECT), 0, k, 0);
mj_local2Global(d, pos, 0, offset, 0, k, 0);
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0);