chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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_;
};