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

Add mjmPair to C API.

Browse Source 
PiperOrigin-RevId: 606274243
Change-Id: I15e2a38096a61bc1377a666c96113ae20ce3ed59
This commit is contained in:
Alessio Quaglino
2024-02-12 09:24:39 -08:00
committed by Copybara-Service
parent 72cf304e2b
commit 13d6eeacdd
8 changed files with 137 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+11 -1
View File
@@ -137,7 +137,7 @@ mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) {
// Add material to model.
// add material to model
mjmMaterial* mjm_addMaterial(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCDef* def = static_cast<mjCDef*>(defspec);
@@ -147,6 +147,16 @@ mjmMaterial* mjm_addMaterial(void* model, void* defspec) {
// add pair to model
mjmPair* mjm_addPair(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCDef* def = static_cast<mjCDef*>(defspec);
mjCPair* pair = modelC->AddPair(def);
return &pair->spec;
}
// add equality to model
mjmEquality* mjm_addEquality(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
src/user/user_api.h
+25
View File
@@ -325,6 +325,25 @@ typedef struct _mjmMaterial { // material specification
} mjmMaterial;
typedef struct _mjmPair {
mjElement element; // internal, do not modify
mjString name; // name
mjString classname; // class name
mjString geomname1; // name of geom 1
mjString geomname2; // name of geom 2
// optional parameters: computed from geoms if not set by user
int condim; // contact dimensionality
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
double friction[5]; // full contact friction
mjString info; // message appended to errors
} mjmPair;
typedef struct _mjmEquality { // equality specification
mjElement element; // internal, do not modify
mjString name; // name
@@ -492,6 +511,9 @@ MJAPI mjmFlex* mjm_addFlex(void* model);
// Add material to model.
MJAPI mjmMaterial* mjm_addMaterial(void* model, void* defspec);
// Add pair to model.
MJAPI mjmPair* mjm_addPair(void* model, void* defspec);
// Add equality to model.
MJAPI mjmEquality* mjm_addEquality(void* model, void* defspec);
@@ -597,6 +619,9 @@ MJAPI void mjm_defaultFlex(mjmFlex& flex);
// Default material attributes.
MJAPI void mjm_defaultMaterial(mjmMaterial& material);
// Default pair attributes.
MJAPI void mjm_defaultPair(mjmPair& pair);
// Default equality attributes.
MJAPI void mjm_defaultEquality(mjmEquality& equality);
src/user/user_init.cc
+14
View File
@@ -205,6 +205,20 @@ void mjm_defaultMaterial(mjmMaterial& material) {
// default pair attributes
void mjm_defaultPair(mjmPair& pair) {
memset(&pair, 0, sizeof(mjmPair));
pair.condim = 3;
mj_defaultSolRefImp(pair.solref, pair.solimp);
pair.friction[0] = 1;
pair.friction[1] = 1;
pair.friction[2] = 0.005;
pair.friction[3] = 0.0001;
pair.friction[4] = 0.0001;
}
// default equality attributes
void mjm_defaultEquality(mjmEquality& equality) {
memset(&equality, 0, sizeof(mjmEquality));
src/user/user_objects.cc
+41 -20
View File
@@ -3341,20 +3341,11 @@ void mjCMaterial::Compile(void) {
// constructor
mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) {
// set defaults
geomname1.clear();
geomname2.clear();
mjm_defaultPair(spec);
condim = 3;
mj_defaultSolRefImp(solref, solimp);
mju_zero(solreffriction, mjNREF);
margin = 0;
gap = 0;
friction[0] = 1;
friction[1] = 1;
friction[2] = 0.005;
friction[3] = 0.0001;
friction[4] = 0.0001;
// set defaults
spec_geomname1_.clear();
spec_geomname2_.clear();
// clear internal variables
geom1 = nullptr;
@@ -3363,33 +3354,63 @@ mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) {
// reset to default if given
if (_def) {
_def->pair.CopyFromSpec();
*this = _def->pair;
}
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
// point to local (needs to be after defaults)
PointToLocal();
// in case this camera is not compiled
CopyFromSpec();
}
void mjCPair::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.geomname1 = (mjString)&spec_geomname1_;
spec.geomname2 = (mjString)&spec_geomname2_;
spec.info = (mjString)&info;
}
void mjCPair::CopyFromSpec() {
*static_cast<mjmPair*>(this) = spec;
geomname1_ = spec_geomname1_;
geomname2_ = spec_geomname2_;
geomname1 = (mjString)&geomname1_;
geomname2 = (mjString)&geomname2_;
}
// compiler
void mjCPair::Compile(void) {
CopyFromSpec();
// check condim
if (condim!=1 && condim!=3 && condim!=4 && condim!=6) {
throw mjCError(this, "invalid condim in collision %d", "", id);
}
// find geom 1
geom1 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname1);
geom1 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname1_);
if (!geom1) {
throw mjCError(this, "geom '%s' not found in collision %d", geomname1.c_str(), id);
throw mjCError(this, "geom '%s' not found in collision %d", geomname1_.c_str(), id);
}
// find geom 2
geom2 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname2);
geom2 = (mjCGeom*)model->FindObject(mjOBJ_GEOM, geomname2_);
if (!geom2) {
throw mjCError(this, "geom '%s' not found in collision %d", geomname2.c_str(), id);
throw mjCError(this, "geom '%s' not found in collision %d", geomname2_.c_str(), id);
}
// mark geoms as not visual
@@ -3398,9 +3419,9 @@ void mjCPair::Compile(void) {
// swap if body1 > body2
if (geom1->body->id > geom2->body->id) {
string nametmp = geomname1;
geomname1 = geomname2;
geomname2 = nametmp;
string nametmp = geomname1_;
geomname1_ = geomname2_;
geomname2_ = nametmp;
mjCGeom* geomtmp = geom1;
geom1 = geom2;
src/user/user_objects.h
+16 -12
View File
@@ -966,24 +966,23 @@ class mjCMaterial : public mjCBase, private mjmMaterial {
//------------------------- class mjCPair ----------------------------------------------------------
// Predefined geom pair for collision detection
class mjCPair : public mjCBase {
class mjCPair : public mjCBase, private mjmPair {
friend class mjCDef;
friend class mjCBody;
friend class mjCModel;
friend class mjXWriter;
public:
// parameters set by user
std::string geomname1; // name of geom 1
std::string geomname2; // name of geom 2
mjmPair spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
// optional parameters: computed from geoms if not set by user
int condim; // contact dimensionality
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
double friction[5]; // full contact friction
void CopyFromSpec();
void PointToLocal();
std::string get_geomname1() { return geomname1_; }
std::string get_geomname2() { return geomname2_; }
int GetSignature(void) {
return signature;
@@ -996,6 +995,11 @@ class mjCPair : public mjCBase {
mjCGeom* geom1; // geom1
mjCGeom* geom2; // geom2
int signature; // body1<<16 + body2
std::string geomname1_;
std::string geomname2_;
std::string spec_geomname1_;
std::string spec_geomname2_;
};
src/xml/xml_native_reader.cc
+20 -9
View File
@@ -1751,18 +1751,26 @@ void mjXReader::OneLight(XMLElement* elem, mjmLight* plight) {
// pair element parser
void mjXReader::OnePair(XMLElement* elem, mjCPair* ppair) {
string text;
void mjXReader::OnePair(XMLElement* elem, mjmPair* ppair) {
string text, name, classname, geomname1, geomname2;
// regular only
if (!readingdefaults) {
ReadAttrTxt(elem, "class", ppair->classname);
ReadAttrTxt(elem, "geom1", ppair->geomname1, true);
ReadAttrTxt(elem, "geom2", ppair->geomname2, true);
if (ReadAttrTxt(elem, "class", classname)) {
mjm_setString(ppair->classname, classname.c_str());
}
if (ReadAttrTxt(elem, "geom1", geomname1)) {
mjm_setString(ppair->geomname1, geomname1.c_str());
}
if (ReadAttrTxt(elem, "geom2", geomname2)) {
mjm_setString(ppair->geomname2, geomname2.c_str());
}
}
// read other parameters
ReadAttrTxt(elem, "name", ppair->name);
if (ReadAttrTxt(elem, "name", name)) {
mjm_setString(ppair->name, name.c_str());
}
ReadAttrInt(elem, "condim", &ppair->condim);
ReadAttr(elem, "solref", mjNREF, ppair->solref, text, false, false);
ReadAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, text, false, false);
@@ -1771,7 +1779,9 @@ void mjXReader::OnePair(XMLElement* elem, mjCPair* ppair) {
ReadAttr(elem, "gap", 1, &ppair->gap, text);
ReadAttr(elem, "friction", 5, ppair->friction, text, false, false);
GetXMLPos(elem, ppair);
// write error info
mjm_setString(ppair->info,
std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str());
}
@@ -2626,7 +2636,7 @@ void mjXReader::Default(XMLElement* section, int parentid) {
else if (name=="light") OneLight(elem, &def->light.spec);
// read pair
else if (name=="pair") OnePair(elem, &def->pair);
else if (name=="pair") OnePair(elem, &def->pair.spec);
// read equality
else if (name=="equality") OneEquality(elem, &def->equality.spec);
@@ -2658,6 +2668,7 @@ void mjXReader::Default(XMLElement* section, int parentid) {
mjm_finalize(def->equality.spec.element);
mjm_finalize(def->tendon.spec.element);
mjm_finalize(def->flex.spec.element);
mjm_finalize(def->pair.spec.element);
// advance
elem = NextSiblingElement(elem);
@@ -3338,7 +3349,7 @@ void mjXReader::Contact(XMLElement* section) {
// geom pair to include
if (name=="pair") {
// create pair and parse
mjCPair* ppair = model->AddPair(def);
mjmPair* ppair = mjm_addPair(model, def);
OnePair(elem, ppair);
}
src/xml/xml_native_reader.h
+1 -1
View File
@@ -62,7 +62,7 @@ class mjXReader : public mjXBase {
void OneSite(tinyxml2::XMLElement* elem, mjmSite& site);
void OneCamera(tinyxml2::XMLElement* elem, mjmCamera* pcamera);
void OneLight(tinyxml2::XMLElement* elem, mjmLight* plight);
void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair);
void OnePair(tinyxml2::XMLElement* elem, mjmPair* ppair);
void OneEquality(tinyxml2::XMLElement* elem, mjmEquality* pequality);
void OneTendon(tinyxml2::XMLElement* elem, mjmTendon* ptendon);
void OneActuator(tinyxml2::XMLElement* elem, mjmActuator* pactuator);
src/xml/xml_native_writer.cc
+9 -9
View File
@@ -523,19 +523,19 @@ void mjXWriter::OnePair(XMLElement* elem, mjCPair* ppair, mjCDef* def) {
// regular
if (!writingdefaults) {
WriteAttrTxt(elem, "class", ppair->classname);
WriteAttrTxt(elem, "geom1", ppair->geomname1);
WriteAttrTxt(elem, "geom2", ppair->geomname2);
WriteAttrTxt(elem, "geom1", ppair->get_geomname1());
WriteAttrTxt(elem, "geom2", ppair->get_geomname2());
}
// defaults and regular
WriteAttrTxt(elem, "name", ppair->name);
WriteAttrInt(elem, "condim", ppair->condim, def->pair.condim);
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);
WriteAttrInt(elem, "condim", ppair->condim, def->pair.spec.condim);
WriteAttr(elem, "margin", 1, &ppair->margin, &def->pair.spec.margin);
WriteAttr(elem, "gap", 1, &ppair->gap, &def->pair.spec.gap);
WriteAttr(elem, "solref", mjNREF, ppair->solref, def->pair.spec.solref);
WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->pair.spec.solreffriction);
WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->pair.spec.solimp);
WriteAttr(elem, "friction", 5, ppair->friction, def->pair.spec.friction);
}