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

Add mjContact.solreffriction, mjModel.pair_solreffriction and related implementation, allowing contact reference acceleration to be different for normal and frictional contact directions. This is required for e.g. elastic frictional collisions.

Browse Source 
PiperOrigin-RevId: 538513357
Change-Id: Id9eb910fe791cf898c712030cb2c0fc481389c91
This commit is contained in:
Yuval Tassa
2023-06-07 09:39:28 -07:00
committed by Copybara-Service
parent 83e70954d7
commit d82f5ce5a4
19 changed files with 187 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+8 -1
View File
@@ -847,6 +847,7 @@ endbroad:
void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, mjtNum usermargin) {
int num, type1, type2, condim;
mjtNum margin, gap, mix, friction[5], solref[mjNREF], solimp[mjNIMP];
mjtNum solreffriction[mjNREF] = {0};
mjContact con[mjMAXCONPAIR];
int ipair = (g2 < 0 ? g1 : -1);
@@ -1045,9 +1046,14 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
friction[i] = m->pair_friction[5*ipair+i];
}
// reference
// reference, normal direction
mju_copy(solref, m->pair_solref+mjNREF*ipair, mjNREF);
// reference, friction directions
if (m->pair_solreffriction[mjNREF*ipair] || m->pair_solreffriction[mjNREF*ipair + 1]) {
mju_copy(solreffriction, m->pair_solreffriction+mjNREF*ipair, mjNREF);
}
// impedance
mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP);
}
@@ -1069,6 +1075,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
con[i].includemargin = margin-gap;
mju_copy(con[i].friction, friction, 5);
mj_assignRef(m, con[i].solref, solref);
mj_assignRef(m, con[i].solreffriction, solreffriction);
mj_assignImp(m, con[i].solimp, solimp);
// exclude in gap
src/engine/engine_core_constraint.c
+34 -14
View File
@@ -993,10 +993,14 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
// get solref, solimp for specified constraint
static void getsolparam(const mjModel* m, const mjData* d, int i, mjtNum* solref, mjtNum* solimp) {
static void getsolparam(const mjModel* m, const mjData* d, int i,
mjtNum* solref, mjtNum* solreffriction, mjtNum* solimp) {
// get constraint id
int id = d->efc_id[i];
// clear solreffriction (applies only to contacts)
mju_zero(solreffriction, mjNREF);
// extract solver parameters from corresponding model element
switch (d->efc_type[i]) {
case mjCNSTR_EQUALITY:
@@ -1028,6 +1032,7 @@ static void getsolparam(const mjModel* m, const mjData* d, int i, mjtNum* solref
case mjCNSTR_CONTACT_PYRAMIDAL:
case mjCNSTR_CONTACT_ELLIPTIC:
mju_copy(solref, d->contact[id].solref, mjNREF);
mju_copy(solreffriction, d->contact[id].solreffriction, mjNREF);
mju_copy(solimp, d->contact[id].solimp, mjNIMP);
}
@@ -1042,6 +1047,17 @@ static void getsolparam(const mjModel* m, const mjData* d, int i, mjtNum* solref
solref[0] = mju_max(solref[0], 2*m->opt.timestep);
}
// check reference format: standard or direct, cannot be mixed
if ((solreffriction[0] > 0) ^ (solreffriction[1] > 0)) {
mju_warning("solreffriction values should have the same sign, replacing with default");
mju_zero(solreffriction, mjNREF); // default solreffriction is (0, 0)
}
// integrator safety: impose ref[0]>=2*timestep for standard format
if (!mjDISABLED(mjDSBL_REFSAFE) && solreffriction[0] > 0) {
solreffriction[0] = mju_max(solreffriction[0], 2*m->opt.timestep);
}
// enforce constraints on solimp
solimp[0] = mju_min(mjMAXIMP, mju_max(mjMINIMP, solimp[0]));
solimp[1] = mju_min(mjMAXIMP, mju_max(mjMINIMP, solimp[1]));
@@ -1149,12 +1165,12 @@ static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin,
void mj_makeImpedance(const mjModel* m, mjData* d) {
int dim, nefc = d->nefc;
mjtNum *R = d->efc_R, *KBIP = d->efc_KBIP;
mjtNum pos, imp, impP, Rpy, solref[mjNREF], solimp[mjNIMP];
mjtNum pos, imp, impP, Rpy, solref[mjNREF], solreffriction[mjNREF], solimp[mjNIMP];
// set efc_R, efc_KBIP
for (int i=0; i < nefc; i++) {
// get solref and solimp
getsolparam(m, d, i, solref, solimp);
getsolparam(m, d, i, solref, solreffriction, solimp);
// get pos and dim
getposdim(m, d, i, &pos, &dim);
@@ -1167,32 +1183,36 @@ void mj_makeImpedance(const mjModel* m, mjData* d) {
// R = (1-imp)/imp * diagApprox
R[i+j] = mju_max(mjMINVAL, (1-imp)*d->efc_diagApprox[i+j]/imp);
// friction: K = 0
// constraint type
int tp = d->efc_type[i+j];
if (tp == mjCNSTR_FRICTION_DOF ||
tp == mjCNSTR_FRICTION_TENDON ||
(tp == mjCNSTR_CONTACT_ELLIPTIC && j > 0)) {
// elliptic contacts use solreffriction in non-normal directions, if non-zero
int elliptic_friction = (tp == mjCNSTR_CONTACT_ELLIPTIC) && (j > 0);
mjtNum* ref = elliptic_friction && (solreffriction[0] || solreffriction[1]) ?
solreffriction : solref;
// friction: K = 0
if (tp == mjCNSTR_FRICTION_DOF || tp == mjCNSTR_FRICTION_TENDON || elliptic_friction) {
KBIP[4*(i+j)] = 0;
}
// standard: K = 1 / (dmax^2 * timeconst^2 * dampratio^2)
else if (solref[0] > 0)
KBIP[4*(i+j)] = 1 / mju_max(mjMINVAL,
solimp[1]*solimp[1] * solref[0]*solref[0] * solref[1]*solref[1]);
else if (ref[0] > 0)
KBIP[4*(i+j)] = 1 / mju_max(mjMINVAL, solimp[1]*solimp[1] * ref[0]*ref[0] * ref[1]*ref[1]);
// direct: K = -solref[0] / dmax^2
else {
KBIP[4*(i+j)] = -solref[0] / mju_max(mjMINVAL, solimp[1]*solimp[1]);
KBIP[4*(i+j)] = -ref[0] / mju_max(mjMINVAL, solimp[1]*solimp[1]);
}
// standard: B = 2 / (dmax*timeconst)
if (solref[1] > 0) {
KBIP[4*(i+j)+1] = 2 / mju_max(mjMINVAL, solimp[1]*solref[0]);
if (ref[1] > 0) {
KBIP[4*(i+j)+1] = 2 / mju_max(mjMINVAL, solimp[1]*ref[0]);
}
// direct: B = -solref[1] / dmax
else {
KBIP[4*(i+j)+1] = -solref[1] / mju_max(mjMINVAL, solimp[1]);
KBIP[4*(i+j)+1] = -ref[1] / mju_max(mjMINVAL, solimp[1]);
}
// I = imp, P = imp'