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

Add mjmEquality, mjmTendon and mjmWrap to C API.

Browse Source 
PiperOrigin-RevId: 605853716
Change-Id: Ieb468cdec5a3032200cf1121960408a61bb08fc5
This commit is contained in:
Alessio Quaglino
2024-02-10 03:29:22 -08:00
committed by Copybara-Service
parent 4ad1dc4b84
commit 1bd6a32af2
11 changed files with 423 additions and 175 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+56
View File
@@ -123,6 +123,62 @@ void* mjm_addFrame(mjmBody* bodyspec, void* parentframe) {
// add equality to model
mjmEquality* mjm_addEquality(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCDef* def = static_cast<mjCDef*>(defspec);
mjCEquality* equality = modelC->AddEquality(def);
return &equality->spec;
}
// add tendon to model
mjmTendon* mjm_addTendon(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCDef* def = static_cast<mjCDef*>(defspec);
mjCTendon* tendon = modelC->AddTendon(def);
return &tendon->spec;
}
// wrap site using tendon
MJAPI mjmWrap* mjm_wrapSite(mjmTendon* tendonspec, const char* name) {
mjCTendon* tendon = reinterpret_cast<mjCTendon*>(tendonspec->element);
tendon->WrapSite(name);
return &tendon->path.back()->spec;
}
// wrap geom using tendon
mjmWrap* mjm_wrapGeom(mjmTendon* tendonspec, const char* name, const char* sidesite) {
mjCTendon* tendon = reinterpret_cast<mjCTendon*>(tendonspec->element);
tendon->WrapGeom(name, sidesite);
return &tendon->path.back()->spec;
}
// wrap joint using tendon
mjmWrap* mjm_wrapJoint(mjmTendon* tendonspec, const char* name, double coef) {
mjCTendon* tendon = reinterpret_cast<mjCTendon*>(tendonspec->element);
tendon->WrapJoint(name, coef);
return &tendon->path.back()->spec;
}
// wrap pulley using tendon
mjmWrap* mjm_wrapPulley(mjmTendon* tendonspec, double divisor) {
mjCTendon* tendon = reinterpret_cast<mjCTendon*>(tendonspec->element);
tendon->WrapPulley(divisor);
return &tendon->path.back()->spec;
}
// add actuator to model
mjmActuator* mjm_addActuator(void* model, void* defspec) {
mjCModel* modelC = static_cast<mjCModel*>(model);
src/user/user_api.h
+71
View File
@@ -211,6 +211,53 @@ typedef struct _mjmLight {
} mjmLight;
typedef struct _mjmEquality {
mjElement element; // compiler only, do not modify
mjString name; // name
mjString classname; // class name
mjtEq type; // constraint type
mjString name1; // name of object 1
mjString name2; // name of object 2
mjtByte active; // initial activation state
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double data[mjNEQDATA]; // type-dependent data
mjString info; // message appended to errors
} mjmEquality;
typedef struct _mjmTendon {
mjElement element; // compiler only, do not modify
mjString name; // name
mjString classname; // class name
int group; // group for visualization
int limited; // does tendon have limits: 0 false, 1 true, 2 auto
double width; // width for rendering
mjtNum solref_limit[mjNREF]; // solver reference: tendon limits
mjtNum solimp_limit[mjNIMP]; // solver impedance: tendon limits
mjtNum solref_friction[mjNREF]; // solver reference: tendon friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction
double range[2]; // length limits
double margin; // margin value for tendon limit detection
double stiffness; // stiffness coefficient
double damping; // damping coefficient
double frictionloss; // friction loss
double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring
mjString material; // name of material for rendering
mjDouble userdata; // user data
float rgba[4]; // rgba when material is omitted
mjString info; // message appended to errors
} mjmTendon;
typedef struct _mjmWrap {
mjElement element; // compiler only, do not modify
mjString name; // name
mjString classname; // class name
mjString info; // message appended to errors
} mjmWrap;
typedef struct _mjmActuator {
mjElement element; // compiler only, do not modify
mjString name; // name
@@ -299,6 +346,24 @@ MJAPI mjmLight* mjm_addLight(mjmBody* body, void* defspec);
// Add frame to body.
MJAPI void* mjm_addFrame(mjmBody* body, void* parentframe);
// Add equality to model.
MJAPI mjmEquality* mjm_addEquality(void* model, void* defspec);
// Add tendon to model.
MJAPI mjmTendon* mjm_addTendon(void* model, void* defspec);
// Wrap site using tendon.
MJAPI mjmWrap* mjm_wrapSite(mjmTendon* tendon, const char* name);
// Wrap geom using tendon.
MJAPI mjmWrap* mjm_wrapGeom(mjmTendon* tendon, const char* name, const char* sidesite);
// Wrap joint using tendon.
MJAPI mjmWrap* mjm_wrapJoint(mjmTendon* tendon, const char* name, double coef);
// Wrap pulley using tendon.
MJAPI mjmWrap* mjm_wrapPulley(mjmTendon* tendon, double divisor);
// Add actuator to model.
MJAPI mjmActuator* mjm_addActuator(void* model, void* defspec);
@@ -365,6 +430,12 @@ MJAPI void mjm_defaultCamera(mjmCamera& camera);
// Default light attributes.
MJAPI void mjm_defaultLight(mjmLight& light);
// Default equality attributes.
MJAPI void mjm_defaultEquality(mjmEquality& equality);
// Default tendon attributes.
MJAPI void mjm_defaultTendon(mjmTendon& tendon);
// Default actuator attributes.
MJAPI void mjm_defaultActuator(mjmActuator& actuator);
src/user/user_composite.cc
+39 -39
View File
@@ -139,7 +139,7 @@ void mjCComposite::SetDefault(void) {
for (int i=0; i<mjNCOMPKINDS; i++) {
def[i].geom.spec.group = 3;
def[i].site.spec.group = 3;
def[i].tendon.group = 3;
def[i].tendon.spec.group = 3;
}
// set default joint
@@ -154,7 +154,7 @@ void mjCComposite::SetDefault(void) {
(type==mjCOMPTYPE_GRID && tmpdim==1)) {
for (int i=0; i<mjNCOMPKINDS; i++) {
def[i].geom.spec.group = 0;
def[i].tendon.group = 0;
def[i].tendon.spec.group = 0;
}
}
@@ -170,8 +170,8 @@ void mjCComposite::SetDefault(void) {
case mjCOMPTYPE_GRID: // grid
// hard main tendon fix
AdjustSoft(def[mjCOMPKIND_TENDON].equality.solref,
def[mjCOMPKIND_TENDON].equality.solimp, 0);
AdjustSoft(def[mjCOMPKIND_TENDON].equality.spec.solref,
def[mjCOMPKIND_TENDON].equality.spec.solimp, 0);
break;
@@ -200,12 +200,12 @@ void mjCComposite::SetDefault(void) {
// soft fix everywhere
for (int i=0; i<mjNCOMPKINDS; i++) {
AdjustSoft(def[i].equality.solref, def[i].equality.solimp, 1);
AdjustSoft(def[i].equality.spec.solref, def[i].equality.spec.solimp, 1);
}
// hard main tendon fix
AdjustSoft(def[mjCOMPKIND_TENDON].equality.solref,
def[mjCOMPKIND_TENDON].equality.solimp, 0);
AdjustSoft(def[mjCOMPKIND_TENDON].equality.spec.solref,
def[mjCOMPKIND_TENDON].equality.spec.solimp, 0);
break;
default:
// SHOULD NOT OCCUR
@@ -535,18 +535,18 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int
mju::sprintf_arr(txt2, "%sS%d", prefix.c_str(), v1);
// create tendon
mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON);
ten->def = model->defaults[0];
ten->name = txt0;
mjmTendon* ten = mjm_addTendon(model, def + mjCOMPKIND_TENDON);
mjm_setDefault(ten->element, model->defaults[0]);
mjm_setString(ten->name, txt0);
ten->group = 4;
ten->WrapSite(txt1);
ten->WrapSite(txt2);
mjm_wrapSite(ten, txt1);
mjm_wrapSite(ten, txt2);
// add equality constraint
mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_TENDON);
eq->def = model->defaults[0];
mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_TENDON);
mjm_setDefault(eq->element, model->defaults[0]);
eq->type = mjEQ_TENDON;
eq->name1 = ten->name;
mjm_setString(eq->name1, mjm_getString(ten->name));
}
}
@@ -657,10 +657,10 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int err
ten->WrapSite(txt2);
// add equality constraint
mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_TENDON);
eq->def = model->defaults[0];
mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_TENDON);
mjm_setDefault(eq->element, model->defaults[0]);
eq->type = mjEQ_TENDON;
eq->name1 = ten->name;
mjm_setString(eq->name1, ten->name.c_str());
}
}
}
@@ -928,12 +928,12 @@ bool mjCComposite::MakeRope(mjCModel* model, mjmBody* body, char* error, int err
char txt2[200];
// add equality constraint
mjCEquality* eq = model->AddEquality();
mjmEquality* eq = mjm_addEquality(model, 0);
eq->type = mjEQ_CONNECT;
mju::sprintf_arr(txt, "%sB0", prefix.c_str());
mju::sprintf_arr(txt2, "%sB%d", prefix.c_str(), count[0]-1);
eq->name1 = txt;
eq->name2 = txt2;
mjm_setString(eq->name1, txt);
mjm_setString(eq->name2, txt2);
mjuu_setvec(eq->data, -0.5*spacing, 0, 0);
mju_copy(eq->solref, solrefsmooth, mjNREF);
mju_copy(eq->solimp, solimpsmooth, mjNIMP);
@@ -1019,10 +1019,10 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i
mjuu_setvec(jnt->axis, 1, 0, 0);
// add constraint
mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_TWIST);
eq->def = model->defaults[0];
mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_TWIST);
mjm_setDefault(eq->element, model->defaults[0]);
eq->type = mjEQ_JOINT;
eq->name1 = mjm_getString(jnt->name);
mjm_setString(eq->name1, mjm_getString(jnt->name));
}
// add stretch joint
@@ -1037,10 +1037,10 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i
mjuu_setvec(jnt->axis, 1, 0, 0);
// add constraint
mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_STRETCH);
eq->def = model->defaults[0];
mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_STRETCH);
mjm_setDefault(eq->element, model->defaults[0]);
eq->type = mjEQ_JOINT;
eq->name1 = mjm_getString(jnt->name);
mjm_setString(eq->name1, mjm_getString(jnt->name));
}
return body;
@@ -1158,10 +1158,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
mjuu_setvec(jnt->axis, 0, 0, 1);
// add fix constraint
mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_JOINT);
eq->def = model->defaults[0];
mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_JOINT);
mjm_setDefault(eq->element, model->defaults[0]);
eq->type = mjEQ_JOINT;
eq->name1 = mjm_getString(jnt->name);
mjm_setString(eq->name1, mjm_getString(jnt->name));
// add joint to tendon
ten->WrapJoint(std::string(mjm_getString(jnt->name)), 1);
@@ -1178,12 +1178,12 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
char txt2[200];
mju::sprintf_arr(txt2,
"%sJ%d_%d_%d", prefix.c_str(), ix1, iy1, iz1);
mjCEquality* eqn = model->AddEquality();
mjmEquality* eqn = mjm_addEquality(model, 0);
mju_copy(eqn->solref, solrefsmooth, mjNREF);
mju_copy(eqn->solimp, solimpsmooth, mjNIMP);
eqn->type = mjEQ_JOINT;
eqn->name1 = txt;
eqn->name2 = txt2;
mjm_setString(eqn->name1, txt);
mjm_setString(eqn->name2, txt2);
}
}
}
@@ -1192,10 +1192,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro
}
// finalize fixed tendon
mjCEquality* eqt = model->AddEquality(def + mjCOMPKIND_TENDON);
eqt->def = model->defaults[0];
mjmEquality* eqt = mjm_addEquality(model, def + mjCOMPKIND_TENDON);
mjm_setDefault(eqt->element, model->defaults[0]);
eqt->type = mjEQ_TENDON;
eqt->name1 = ten->name;
mjm_setString(eqt->name1, ten->name.c_str());
// skin
if (skin) {
@@ -1228,10 +1228,10 @@ void mjCComposite::MakeShear(mjCModel* model) {
ten->name = txt;
// equality constraint
mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_SHEAR);
eq->def = model->defaults[0];
mjmEquality* eq = mjm_addEquality(model, def + mjCOMPKIND_SHEAR);
mjm_setDefault(eq->element, model->defaults[0]);
eq->type = mjEQ_TENDON;
eq->name1 = txt;
mjm_setString(eq->name1, txt);
}
}
}
src/user/user_flexcomp.cc
+3 -3
View File
@@ -478,11 +478,11 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
// create edge equality constraint
if (equality) {
mjCEquality *pe = model->AddEquality(&def);
pe->def = model->defaults[0];
mjmEquality* pe = mjm_addEquality(model, &def);
mjm_setDefault(pe->element, model->defaults[0]);
pe->type = mjEQ_FLEX;
pe->active = true;
pe->name1 = name;
mjm_setString(pe->name1, name.c_str());
}
return true;
src/user/user_init.cc
+33
View File
@@ -134,6 +134,39 @@ void mjm_defaultLight(mjmLight& light) {
// Default equality attributes.
void mjm_defaultEquality(mjmEquality& equality) {
memset(&equality, 0, sizeof(mjmEquality));
equality.type = mjEQ_CONNECT;
equality.active = 1;
mj_defaultSolRefImp(equality.solref, equality.solimp);
equality.data[1] = 1;
equality.data[10] = 1; // torque:force ratio
}
// default tendon attributes
void mjm_defaultTendon(mjmTendon& tendon) {
memset(&tendon, 0, sizeof(mjmTendon));
tendon.group = 0;
tendon.width = 0.003;
tendon.limited = 2;
tendon.range[0] = 0;
tendon.range[1] = 0;
mj_defaultSolRefImp(tendon.solref_limit, tendon.solimp_limit);
mj_defaultSolRefImp(tendon.solref_friction, tendon.solimp_friction);
tendon.margin = 0;
tendon.stiffness = 0;
tendon.damping = 0;
tendon.frictionloss = 0;
tendon.springlength[0] = tendon.springlength[1] = -1;
tendon.rgba[0] = tendon.rgba[1] = tendon.rgba[2] = 0.5f;
tendon.rgba[3] = 1.0f;
}
// default actuator attributes
void mjm_defaultActuator(mjmActuator& actuator) {
memset(&actuator, 0, sizeof(mjmActuator));
src/user/user_model.cc
+4 -4
View File
@@ -2022,7 +2022,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// find equality constraint referencing this flex
m->flex_edgeequality[i] = 0;
for (int k=0; k<(int)equalities.size(); k++) {
if (equalities[k]->type==mjEQ_FLEX && equalities[k]->name1==pfl->name) {
if (equalities[k]->type==mjEQ_FLEX && equalities[k]->name1_==pfl->name) {
m->flex_edgeequality[i] = 1;
break;
}
@@ -2251,7 +2251,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss;
m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0];
m->tendon_lengthspring[2*i+1] = (mjtNum)pte->springlength[1];
copyvec(m->tendon_user+nuser_tendon*i, pte->userdata.data(), nuser_tendon);
copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon);
copyvec(m->tendon_rgba+4*i, pte->rgba, 4);
// set wraps
@@ -2914,7 +2914,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
if (nuser_tendon == -1) {
nuser_tendon = 0;
for (int i=0; i<tendons.size(); i++) {
nuser_tendon = mjMAX(nuser_tendon, tendons[i]->userdata.size());
nuser_tendon = mjMAX(nuser_tendon, tendons[i]->spec_userdata_.size());
}
}
if (nuser_actuator == -1) {
@@ -3480,7 +3480,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
tendons[i]->frictionloss = (double)m->tendon_frictionloss[i];
if (nuser_tendon) {
copyvec(tendons[i]->userdata.data(), m->tendon_user + nuser_tendon*i, nuser_tendon);
copyvec(tendons[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon);
}
}
src/user/user_objects.cc
+87 -38
View File
@@ -474,7 +474,7 @@ void mjCDef::Compile(const mjCModel* model) {
geom.userdata_.resize(model->nuser_geom);
site.userdata_.resize(model->nuser_site);
camera.userdata_.resize(model->nuser_cam);
tendon.userdata.resize(model->nuser_tendon);
tendon.userdata_.resize(model->nuser_tendon);
actuator.userdata_.resize(model->nuser_actuator);
}
@@ -3509,33 +3509,55 @@ void mjCBodyPair::Compile(void) {
// initialize default constraint
mjCEquality::mjCEquality(mjCModel* _model, mjCDef* _def) {
// set defaults
type = mjEQ_CONNECT;
name1.clear();
name2.clear();
active = true;
mj_defaultSolRefImp(solref, solimp);
mjuu_zerovec(data, mjNEQDATA);
data[1] = 1;
data[10] = 1; // torque:force ratio
mjm_defaultEquality(spec);
// clear internal variables
spec_name1_.clear();
spec_name2_.clear();
obj1id = obj2id = -1;
// reset to default if given
if (_def) {
_def->equality.CopyFromSpec();
*this = _def->equality;
}
// 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 mjCEquality::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.name1 = (mjString)&spec_name1_;
spec.name2 = (mjString)&spec_name2_;
spec.info = (mjString)&info;
}
void mjCEquality::CopyFromSpec() {
*static_cast<mjmEquality*>(this) = spec;
name1_ = spec_name1_;
name2_ = spec_name2_;
name1 = (mjString)&name1_;
name2 = (mjString)&name2_;
}
// compiler
void mjCEquality::Compile(void) {
CopyFromSpec();
mjtObj objtype;
mjCBase *px1, *px2;
mjtJoint jt1, jt2;
@@ -3554,17 +3576,17 @@ void mjCEquality::Compile(void) {
}
// find object 1, get id
px1 = model->FindObject(objtype, name1);
px1 = model->FindObject(objtype, name1_);
if (!px1) {
throw mjCError(this, "unknown element '%s' in equality constraint %d", name1.c_str(), id);
throw mjCError(this, "unknown element '%s' in equality constraint %d", name1_.c_str(), id);
}
obj1id = px1->id;
// find object 2, get id
if (!name2.empty()) {
px2 = model->FindObject(objtype, name2);
if (!name2_.empty()) {
px2 = model->FindObject(objtype, name2_);
if (!px2) {
throw mjCError(this, "unknown element '%s' in equality constraint %d", name2.c_str(), id);
throw mjCError(this, "unknown element '%s' in equality constraint %d", name2_.c_str(), id);
}
obj2id = px2->id;
}
@@ -3587,12 +3609,12 @@ void mjCEquality::Compile(void) {
// make sure flex is not rigid
if (type==mjEQ_FLEX && model->flexes[obj1id]->rigid) {
throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1.c_str(), id);
throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id);
}
// make sure the two objects are different
if (obj1id==obj2id) {
throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1.c_str(), id);
throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1_.c_str(), id);
}
// make sure joints are scalar
@@ -3613,25 +3635,11 @@ void mjCEquality::Compile(void) {
// constructor
mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) {
// tendon defaults
group = 0;
material_.clear();
width = 0.003;
limited = 2;
range[0] = 0;
range[1] = 0;
mj_defaultSolRefImp(solref_limit, solimp_limit);
mj_defaultSolRefImp(solref_friction, solimp_friction);
margin = 0;
stiffness = 0;
damping = 0;
frictionloss = 0;
springlength[0] = springlength[1] = -1;
rgba[0] = rgba[1] = rgba[2] = 0.5f;
rgba[3] = 1.0f;
userdata.clear();
mjm_defaultTendon(spec);
// clear internal variables
spec_material_.clear();
spec_userdata_.clear();
path.clear();
matid = -1;
@@ -3643,6 +3651,33 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) {
// 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 mjCTendon::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.material = (mjString)&spec_material_;
spec.userdata = (mjDouble)&spec_userdata_;
spec.info = (mjString)&info;
}
void mjCTendon::CopyFromSpec() {
*static_cast<mjmTendon*>(this) = spec;
material_ = spec_material_;
userdata_ = spec_userdata_;
material = (mjString)&material_;
userdata = (mjDouble)&userdata_;
}
@@ -3741,12 +3776,14 @@ mjCWrap* mjCTendon::GetWrap(int id) {
// compiler
void mjCTendon::Compile(void) {
CopyFromSpec();
// resize userdata
if (userdata.size() > model->nuser_tendon) {
if (userdata_.size() > model->nuser_tendon) {
throw mjCError(this, "user has more values than nuser_tendon in tendon '%s' (id = %d)",
name.c_str(), id);
}
userdata.resize(model->nuser_tendon);
userdata_.resize(model->nuser_tendon);
// check for empty path
int sz = (int)path.size();
@@ -3880,6 +3917,18 @@ mjCWrap::mjCWrap(mjCModel* _model, mjCTendon* _tendon) {
sideid = -1;
prm = 0;
sidesite.clear();
// point to local
PointToLocal();
}
void mjCWrap::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.info = (mjString)&info;
}
@@ -4528,7 +4577,7 @@ void mjCSensor::Compile(void) {
}
// make sure tendon has limit
if (!((mjCTendon*)obj)->limited) {
if (!((mjCTendon*)obj)->spec.limited) {
throw mjCError(this, "tendon must be limited in sensor '%s' (id = %d)", name.c_str(), id);
}
src/user/user_objects.h
+39 -29
View File
@@ -1016,21 +1016,20 @@ class mjCBodyPair : public mjCBase {
//------------------------- class mjCEquality ------------------------------------------------------
// Describes an equality constraint
class mjCEquality : public mjCBase {
class mjCEquality : public mjCBase, private mjmEquality {
friend class mjCDef;
friend class mjCBody;
friend class mjCModel;
friend class mjXWriter;
public:
// variables set by user
mjtEq type; // constraint type
std::string name1; // name of object 1
std::string name2; // name of object 2
bool active; // initial activation state
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double data[mjNEQDATA]; // type-dependent data
mjmEquality spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void CopyFromSpec();
void PointToLocal();
private:
mjCEquality(mjCModel* = 0, mjCDef* = 0); // constructor
@@ -1038,6 +1037,11 @@ class mjCEquality : public mjCBase {
int obj1id; // id of object 1
int obj2id; // id of object 2
std::string name1_;
std::string name2_;
std::string spec_name1_;
std::string spec_name2_;
};
@@ -1045,12 +1049,17 @@ class mjCEquality : public mjCBase {
//------------------------- class mjCTendon --------------------------------------------------------
// Describes a tendon
class mjCTendon : public mjCBase {
class mjCTendon : public mjCBase, private mjmTendon {
friend class mjCDef;
friend class mjCModel;
friend class mjXWriter;
public:
mjmTendon spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void set_material(std::string _material) { material_ = _material; }
std::string& get_material() { return material_; }
void del_material() { material_.clear(); }
@@ -1064,32 +1073,26 @@ class mjCTendon : public mjCBase {
// API for access to wrapping objects
int NumWraps(void); // number of wraps
mjCWrap* GetWrap(int); // pointer to wrap
std::vector<mjCWrap*> path; // wrapping objects
// variables set by user
int group; // group for visualization
int limited; // does tendon have limits: 0 false, 1 true, 2 auto
double width; // width for rendering
mjtNum solref_limit[mjNREF]; // solver reference: tendon limits
mjtNum solimp_limit[mjNIMP]; // solver impedance: tendon limits
mjtNum solref_friction[mjNREF]; // solver reference: tendon friction
mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction
double range[2]; // length limits
double margin; // margin value for tendon limit detection
double stiffness; // stiffness coefficient
double damping; // damping coefficient
double frictionloss; // friction loss
double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring
std::vector<double> userdata; // user data
float rgba[4]; // rgba when material is omitted
// used by mjXWriter and mjCModel
const std::vector<double>& get_userdata() { return userdata_; }
void CopyFromSpec();
void PointToLocal();
private:
mjCTendon(mjCModel* = 0, mjCDef* = 0); // constructor
~mjCTendon(); // destructor
void Compile(void); // compiler
std::string material_; // name of material for rendering
int matid; // material id for rendering
std::vector<mjCWrap*> path; // wrapping objects
// variable-size data
std::string material_;
std::string spec_material_;
std::vector<double> userdata_;
std::vector<double> spec_userdata_;
};
@@ -1097,11 +1100,18 @@ class mjCTendon : public mjCBase {
//------------------------- class mjCWrap ----------------------------------------------------------
// Describes a tendon wrap object
class mjCWrap : public mjCBase {
class mjCWrap : public mjCBase, private mjmWrap {
friend class mjCTendon;
friend class mjCModel;
public:
mjmWrap spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void PointToLocal();
mjtWrap type; // wrap object type
mjCBase* obj; // wrap object pointer
int sideid; // side site id; -1 if not applicable
src/xml/xml_native_reader.cc
+78 -49
View File
@@ -1751,9 +1751,9 @@ void mjXReader::OnePair(XMLElement* elem, mjCPair* ppair) {
// equality element parser
void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
void mjXReader::OneEquality(XMLElement* elem, mjmEquality* pequality) {
int n;
string text;
string text, name1, name2, name, classname;
// read type (bad keywords already detected by schema)
text = elem->Value();
@@ -1761,19 +1761,23 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
// regular only
if (!readingdefaults) {
ReadAttrTxt(elem, "name", pequality->name);
ReadAttrTxt(elem, "class", pequality->classname);
if (ReadAttrTxt(elem, "name", name)) {
mjm_setString(pequality->name, name.c_str());
}
if (ReadAttrTxt(elem, "class", classname)) {
mjm_setString(pequality->classname, classname.c_str());
};
switch (pequality->type) {
case mjEQ_CONNECT:
ReadAttrTxt(elem, "body1", pequality->name1, true);
ReadAttrTxt(elem, "body2", pequality->name2);
ReadAttrTxt(elem, "body1", name1, true);
ReadAttrTxt(elem, "body2", name2);
ReadAttr(elem, "anchor", 3, pequality->data, text, true);
break;
case mjEQ_WELD:
ReadAttrTxt(elem, "body1", pequality->name1, true);
ReadAttrTxt(elem, "body2", pequality->name2);
ReadAttrTxt(elem, "body1", name1, true);
ReadAttrTxt(elem, "body2", name2);
ReadAttr(elem, "relpose", 7, pequality->data+3, text);
ReadAttr(elem, "torquescale", 1, pequality->data+10, text);
if (!ReadAttr(elem, "anchor", 3, pequality->data, text)) {
@@ -1782,19 +1786,19 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
break;
case mjEQ_JOINT:
ReadAttrTxt(elem, "joint1", pequality->name1, true);
ReadAttrTxt(elem, "joint2", pequality->name2);
ReadAttrTxt(elem, "joint1", name1, true);
ReadAttrTxt(elem, "joint2", name2);
ReadAttr(elem, "polycoef", 5, pequality->data, text);
break;
case mjEQ_TENDON:
ReadAttrTxt(elem, "tendon1", pequality->name1, true);
ReadAttrTxt(elem, "tendon2", pequality->name2);
ReadAttrTxt(elem, "tendon1", name1, true);
ReadAttrTxt(elem, "tendon2", name2);
ReadAttr(elem, "polycoef", 5, pequality->data, text);
break;
case mjEQ_FLEX:
ReadAttrTxt(elem, "flex", pequality->name1, true);
ReadAttrTxt(elem, "flex", name1, true);
break;
case mjEQ_DISTANCE:
@@ -1804,6 +1808,11 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
default: // SHOULD NOT OCCUR
throw mjXError(elem, "unrecognized equality constraint type");
}
mjm_setString(pequality->name1, name1.c_str());
if (!name2.empty()) {
mjm_setString(pequality->name2, name2.c_str());
}
}
// read attributes
@@ -1813,20 +1822,29 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
ReadAttr(elem, "solref", mjNREF, pequality->solref, text, false, false);
ReadAttr(elem, "solimp", mjNIMP, pequality->solimp, text, false, false);
GetXMLPos(elem, pequality);
// write error info
mjm_setString(pequality->info,
std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str());
}
// tendon element parser
void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) {
string text;
void mjXReader::OneTendon(XMLElement* elem, mjmTendon* pten) {
string text, name, classname, material;
std::vector<double> userdata;
// read attributes
ReadAttrTxt(elem, "name", pten->name);
ReadAttrTxt(elem, "class", pten->classname);
if (ReadAttrTxt(elem, "name", name)) {
mjm_setString(pten->name, name.c_str());
}
if (ReadAttrTxt(elem, "class", classname)) {
mjm_setString(pten->classname, classname.c_str());
}
ReadAttrInt(elem, "group", &pten->group);
ReadAttrTxt(elem, "material", pten->get_material());
if (ReadAttrTxt(elem, "material", material)) {
mjm_setString(pten->material, material.c_str());
}
MapValue(elem, "limited", &pten->limited, TFAuto_map, 3);
ReadAttr(elem, "width", 1, &pten->width, text);
ReadAttr(elem, "solreflimit", mjNREF, pten->solref_limit, text, false, false);
@@ -1845,9 +1863,13 @@ void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) {
ReadAttr(elem, "rgba", 4, pten->rgba, text);
// read userdata
ReadVector(elem, "user", pten->userdata, text);
if (ReadVector(elem, "user", userdata, text)) {
mjm_setDouble(pten->userdata, userdata.data(), userdata.size());
}
GetXMLPos(elem, pten);
// write error info
mjm_setString(pten->info,
std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str());
}
@@ -2280,8 +2302,8 @@ void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) {
ReadAttr(ejnt, "axis", 3, el->joint.spec.axis, text);
// solreffix, solimpfix
ReadAttr(ejnt, "solreffix", mjNREF, el->equality.solref, text, false, false);
ReadAttr(ejnt, "solimpfix", mjNIMP, el->equality.solimp, text, false, false);
ReadAttr(ejnt, "solreffix", mjNREF, el->equality.spec.solref, text, false, false);
ReadAttr(ejnt, "solimpfix", mjNIMP, el->equality.spec.solimp, text, false, false);
// joint attributes
MapValue(elem, "limited", &el->joint.spec.limited, TFAuto_map, 3);
@@ -2312,26 +2334,28 @@ void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) {
comp.add[kind] = true;
// solreffix, solimpfix
ReadAttr(eten, "solreffix", mjNREF, comp.def[kind].equality.solref, text, false, false);
ReadAttr(eten, "solimpfix", mjNIMP, comp.def[kind].equality.solimp, text, false, false);
ReadAttr(eten, "solreffix", mjNREF, comp.def[kind].equality.spec.solref, text, false, false);
ReadAttr(eten, "solimpfix", mjNIMP, comp.def[kind].equality.spec.solimp, text, false, false);
// tendon attributes
MapValue(elem, "limited", &comp.def[kind].tendon.limited, TFAuto_map, 3);
ReadAttrInt(eten, "group", &comp.def[kind].tendon.group);
ReadAttr(eten, "solreflimit", mjNREF, comp.def[kind].tendon.solref_limit, text, false, false);
ReadAttr(eten, "solimplimit", mjNIMP, comp.def[kind].tendon.solimp_limit, text, false, false);
std::string material;
MapValue(elem, "limited", &comp.def[kind].tendon.spec.limited, TFAuto_map, 3);
ReadAttrInt(eten, "group", &comp.def[kind].tendon.spec.group);
ReadAttr(eten, "solreflimit", mjNREF, comp.def[kind].tendon.spec.solref_limit, text, false, false);
ReadAttr(eten, "solimplimit", mjNIMP, comp.def[kind].tendon.spec.solimp_limit, text, false, false);
ReadAttr(eten,
"solreffriction", mjNREF, comp.def[kind].tendon.solref_friction, text, false, false);
"solreffriction", mjNREF, comp.def[kind].tendon.spec.solref_friction, text, false, false);
ReadAttr(eten,
"solimpfriction", mjNIMP, comp.def[kind].tendon.solimp_friction, text, false, false);
ReadAttr(eten, "range", 2, comp.def[kind].tendon.range, text);
ReadAttr(eten, "margin", 1, &comp.def[kind].tendon.margin, text);
ReadAttr(eten, "stiffness", 1, &comp.def[kind].tendon.stiffness, text);
ReadAttr(eten, "damping", 1, &comp.def[kind].tendon.damping, text);
ReadAttr(eten, "frictionloss", 1, &comp.def[kind].tendon.frictionloss, text);
ReadAttrTxt(eten, "material", comp.def[kind].tendon.get_material());
ReadAttr(eten, "rgba", 4, comp.def[kind].tendon.rgba, text);
ReadAttr(eten, "width", 1, &comp.def[kind].tendon.width, text);
"solimpfriction", mjNIMP, comp.def[kind].tendon.spec.solimp_friction, text, false, false);
ReadAttr(eten, "range", 2, comp.def[kind].tendon.spec.range, text);
ReadAttr(eten, "margin", 1, &comp.def[kind].tendon.spec.margin, text);
ReadAttr(eten, "stiffness", 1, &comp.def[kind].tendon.spec.stiffness, text);
ReadAttr(eten, "damping", 1, &comp.def[kind].tendon.spec.damping, text);
ReadAttr(eten, "frictionloss", 1, &comp.def[kind].tendon.spec.frictionloss, text);
ReadAttrTxt(eten, "material", material);
mjm_setString(comp.def[kind].tendon.spec.material, material.c_str());
ReadAttr(eten, "rgba", 4, comp.def[kind].tendon.spec.rgba, text);
ReadAttr(eten, "width", 1, &comp.def[kind].tendon.spec.width, text);
// advance
eten = eten->NextSiblingElement("tendon");
@@ -2417,8 +2441,8 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) {
if (MapValue(edge, "equality", &n, bool_map, 2)) {
fcomp.equality = (n==1);
}
ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.solref, text, false, false);
ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.solimp, text, false, false);
ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.spec.solref, text, false, false);
ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.spec.solimp, text, false, false);
ReadAttr(edge, "stiffness", 1, &fcomp.def.flex.edgestiffness, text);
ReadAttr(edge, "damping", 1, &fcomp.def.flex.edgedamping, text);
}
@@ -2577,10 +2601,10 @@ void mjXReader::Default(XMLElement* section, int parentid) {
else if (name=="pair") OnePair(elem, &def->pair);
// read equality
else if (name=="equality") OneEquality(elem, &def->equality);
else if (name=="equality") OneEquality(elem, &def->equality.spec);
// read tendon
else if (name=="tendon") OneTendon(elem, &def->tendon);
else if (name=="tendon") OneTendon(elem, &def->tendon.spec);
// read actuator
else if (name=="general" ||
@@ -2602,6 +2626,8 @@ void mjXReader::Default(XMLElement* section, int parentid) {
mjm_finalize(def->camera.spec.element);
mjm_finalize(def->light.spec.element);
mjm_finalize(def->actuator.spec.element);
mjm_finalize(def->equality.spec.element);
mjm_finalize(def->tendon.spec.element);
// advance
elem = elem->NextSiblingElement();
@@ -3318,7 +3344,7 @@ void mjXReader::Equality(XMLElement* section) {
}
// create equality constraint and parse
mjCEquality* pequality = model->AddEquality(def);
mjmEquality* pequality = mjm_addEquality(model, def);
OneEquality(elem, pequality);
// advance to next element
@@ -3382,7 +3408,7 @@ void mjXReader::Tendon(XMLElement* section) {
}
// create equality constraint and parse
mjCTendon* pten = model->AddTendon(def);
mjmTendon* pten = mjm_addTendon(model, def);
OneTendon(elem, pten);
// process wrap sub-elements
@@ -3390,11 +3416,12 @@ void mjXReader::Tendon(XMLElement* section) {
while (sub) {
// get wrap type
string wrap = sub->Value();
mjmWrap* pwrap;;
// read attributes depending on type
if (wrap=="site") {
ReadAttrTxt(sub, "site", text, true);
pten->WrapSite(text, "line = " + std::to_string(sub->GetLineNum()));
pwrap = mjm_wrapSite(pten, text.c_str());
}
else if (wrap=="geom") {
@@ -3402,24 +3429,26 @@ void mjXReader::Tendon(XMLElement* section) {
if (!ReadAttrTxt(sub, "sidesite", text1)) {
text1.clear();
}
pten->WrapGeom(text, text1, "line = " + std::to_string(sub->GetLineNum()));
pwrap = mjm_wrapGeom(pten, text.c_str(), text1.c_str());
}
else if (wrap=="pulley") {
ReadAttr(sub, "divisor", 1, &data, text, true);
pten->WrapPulley(data, "line = " + std::to_string(sub->GetLineNum()));
pwrap = mjm_wrapPulley(pten, data);
}
else if (wrap=="joint") {
ReadAttrTxt(sub, "joint", text, true);
ReadAttr(sub, "coef", 1, &data, text1, true);
pten->WrapJoint(text, data, "line = " + std::to_string(sub->GetLineNum()));
pwrap = mjm_wrapJoint(pten, text.c_str(), data);
}
else {
throw mjXError(sub, "unknown wrap type"); // SHOULD NOT OCCUR
}
mjm_setString(pwrap->info, ("line = " + std::to_string(sub->GetLineNum())).c_str());
// advance to next sub-element
sub = sub->NextSiblingElement();
}
src/xml/xml_native_reader.h
+2 -2
View File
@@ -63,8 +63,8 @@ class mjXReader : public mjXBase {
void OneCamera(tinyxml2::XMLElement* elem, mjmCamera* pcamera);
void OneLight(tinyxml2::XMLElement* elem, mjmLight* plight);
void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair);
void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality);
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon);
void OneEquality(tinyxml2::XMLElement* elem, mjmEquality* pequality);
void OneTendon(tinyxml2::XMLElement* elem, mjmTendon* ptendon);
void OneActuator(tinyxml2::XMLElement* elem, mjmActuator* pactuator);
void OneComposite(tinyxml2::XMLElement* elem, mjmBody* pbody, mjCDef* def);
void OneFlexcomp(tinyxml2::XMLElement* elem, mjmBody* pbody);
src/xml/xml_native_writer.cc
+11 -11
View File
@@ -559,33 +559,33 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
switch (peq->type) {
case mjEQ_CONNECT:
WriteAttrTxt(elem, "body1", peq->name1);
WriteAttrTxt(elem, "body2", peq->name2);
WriteAttrTxt(elem, "body1", mjm_getString(peq->name1));
WriteAttrTxt(elem, "body2", mjm_getString(peq->name2));
WriteAttr(elem, "anchor", 3, peq->data);
break;
case mjEQ_WELD:
WriteAttrTxt(elem, "body1", peq->name1);
WriteAttrTxt(elem, "body2", peq->name2);
WriteAttrTxt(elem, "body1", mjm_getString(peq->name1));
WriteAttrTxt(elem, "body2", mjm_getString(peq->name2));
WriteAttr(elem, "anchor", 3, peq->data);
WriteAttr(elem, "torquescale", 1, peq->data+10);
WriteAttr(elem, "relpose", 7, peq->data+3);
break;
case mjEQ_JOINT:
WriteAttrTxt(elem, "joint1", peq->name1);
WriteAttrTxt(elem, "joint2", peq->name2);
WriteAttrTxt(elem, "joint1", mjm_getString(peq->name1));
WriteAttrTxt(elem, "joint2", mjm_getString(peq->name2));
WriteAttr(elem, "polycoef", 5, peq->data);
break;
case mjEQ_TENDON:
WriteAttrTxt(elem, "tendon1", peq->name1);
WriteAttrTxt(elem, "tendon2", peq->name2);
WriteAttrTxt(elem, "tendon1", mjm_getString(peq->name1));
WriteAttrTxt(elem, "tendon2", mjm_getString(peq->name2));
WriteAttr(elem, "polycoef", 5, peq->data);
break;
case mjEQ_FLEX:
WriteAttrTxt(elem, "flex", peq->name1);
WriteAttrTxt(elem, "flex", mjm_getString(peq->name1));
break;
default:
@@ -646,9 +646,9 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
// userdata
if (writingdefaults) {
WriteVector(elem, "user", pten->userdata);
WriteVector(elem, "user", pten->get_userdata());
} else {
WriteVector(elem, "user", pten->userdata, def->tendon.userdata);
WriteVector(elem, "user", pten->get_userdata(), def->tendon.get_userdata());
}
}