chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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'
src/user/user_model.cc
+2
View File
@@ -1871,6 +1871,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->pair_geom2[i] = pairs[i]->geom2;
m->pair_signature[i] = pairs[i]->signature;
copyvec(m->pair_solref+mjNREF*i, pairs[i]->solref, mjNREF);
copyvec(m->pair_solreffriction+mjNREF*i, pairs[i]->solreffriction, mjNREF);
copyvec(m->pair_solimp+mjNIMP*i, pairs[i]->solimp, mjNIMP);
m->pair_margin[i] = (mjtNum)pairs[i]->margin;
m->pair_gap[i] = (mjtNum)pairs[i]->gap;
@@ -3037,6 +3038,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
// pairs
for (int i=0; i<npair; i++) {
copyvec(pairs[i]->solref, m->pair_solref+mjNREF*i, mjNREF);
copyvec(pairs[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF);
copyvec(pairs[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP);
pairs[i]->margin = (double)m->pair_margin[i];
pairs[i]->gap = (double)m->pair_gap[i];
src/user/user_objects.cc
+1
View File
@@ -2931,6 +2931,7 @@ mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) {
condim = 3;
mj_defaultSolRefImp(solref, solimp);
mju_zero(solreffriction, mjNREF);
margin = 0;
gap = 0;
friction[0] = 1;
src/user/user_objects.h
+2 -1
View File
@@ -772,7 +772,8 @@ class mjCPair : public mjCBase {
// optional parameters: computed from geoms if not set by user
int condim; // contact dimensionality
mjtNum solref[mjNREF]; // solver reference
mjtNum solref[mjNREF]; // solver reference, normal direction
mjtNum solreffriction[mjNREF]; // solver reference, frictional directions
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
src/xml/xml_native_reader.cc
+5 -3
View File
@@ -153,7 +153,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"axisangle", "xyaxes", "zaxis", "euler", "mode", "user"},
{"light", "?", "12", "pos", "dir", "directional", "castshadow", "active",
"attenuation", "cutoff", "exponent", "ambient", "diffuse", "specular", "mode"},
{"pair", "?", "6", "condim", "friction", "solref", "solimp", "gap", "margin"},
{"pair", "?", "7", "condim", "friction", "solref", "solreffriction", "solimp",
"gap", "margin"},
{"equality", "?", "3", "active", "solref", "solimp"},
{"tendon", "?", "16", "group", "limited", "range",
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
@@ -283,8 +284,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"contact", "*", "0"},
{"<"},
{"pair", "*", "10", "name", "class", "geom1", "geom2", "condim", "friction",
"solref", "solimp", "gap", "margin"},
{"pair", "*", "11", "name", "class", "geom1", "geom2", "condim", "friction",
"solref", "solreffriction", "solimp", "gap", "margin"},
{"exclude", "*", "3", "name", "body1", "body2"},
{">"},
@@ -1495,6 +1496,7 @@ void mjXReader::OnePair(XMLElement* elem, mjCPair* ppair) {
ReadAttrTxt(elem, "name", ppair->name);
ReadAttrInt(elem, "condim", &ppair->condim);
ReadAttr(elem, "solref", mjNREF, ppair->solref, text, false, false);
ReadAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, text, false, false);
ReadAttr(elem, "solimp", mjNIMP, ppair->solimp, text, false, false);
ReadAttr(elem, "margin", 1, &ppair->margin, text);
ReadAttr(elem, "gap", 1, &ppair->gap, text);
src/xml/xml_native_writer.cc
+1
View File
@@ -443,6 +443,7 @@ void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) {
WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.margin);
WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.gap);
WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.solref);
WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.solreffriction);
WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.solimp);
WriteAttr(elem, "friction", 5, ppair->friction, def->pair.friction);
}