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

Add mjmNumeric, mjmText, mjmTuple, mjmKey to C API.

Browse Source 
PiperOrigin-RevId: 606919327
Change-Id: I477f6cd8beefcb3962238722565e3b398b73446c
This commit is contained in:
Alessio Quaglino
2024-02-14 03:09:38 -08:00
committed by Copybara-Service
parent 266161ee4c
commit 2ee8a4dabd
9 changed files with 482 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+36
View File
@@ -251,6 +251,42 @@ mjmSensor* mjm_addSensor(void* model) {
// add numeric to model
mjmNumeric* mjm_addNumeric(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCNumeric* numeric = modelC->AddNumeric();
return &numeric->spec;
}
// add text to model
mjmText* mjm_addText(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCText* text = modelC->AddText();
return &text->spec;
}
// add tuple to model
mjmTuple* mjm_addTuple(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCTuple* tuple = modelC->AddTuple();
return &tuple->spec;
}
// add keyframe to model
mjmKey* mjm_addKey(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCKey* key = modelC->AddKey();
return &key->spec;
}
// Add plugin to model. // Add plugin to model.
mjElement mjm_addPlugin(void* model) { mjElement mjm_addPlugin(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model); mjCModel* modelC = static_cast<mjCModel*>(model);
src/user/user_api.h
+65
View File
@@ -541,6 +541,47 @@ typedef struct _mjmSensor { // sensor specfication
} mjmSensor; } mjmSensor;
typedef struct _mjmNumeric { // custom numeric field specification
mjElement element; // internal, do not modify
mjString name; // name
mjDoubleVec data; // initialization data
int size; // array size, can be bigger than data size
mjString info; // message appended to compiler errors
} mjmNumeric;
typedef struct _mjmText { // custom text specification
mjElement element; // internal, do not modify
mjString name; // name
mjString data; // text string
mjString info; // message appended to compiler errors
} mjmText;
typedef struct _mjmTuple { // tuple specification
mjElement element; // internal, do not modify
mjString name; // name
mjIntVec objtype; // object types
mjStringVec objname; // object names
mjDoubleVec objprm; // object parameters
mjString info; // message appended to compiler errors
} mjmTuple;
typedef struct _mjmKey { // keyframe specification
mjElement element; // internal, do not modify
mjString name; // name
double time; // time
mjDoubleVec qpos; // qpos
mjDoubleVec qvel; // qvel
mjDoubleVec act; // act
mjDoubleVec mpos; // mocap pos
mjDoubleVec mquat; // mocap quat
mjDoubleVec ctrl; // ctrl
mjString info; // message appended to compiler errors
} mjmKey;
//---------------------------------- API functions ------------------------------------------------- //---------------------------------- API functions -------------------------------------------------
// Create model. // Create model.
@@ -615,6 +656,18 @@ MJAPI mjmActuator* mjm_addActuator(void* model, void* defspec);
// Add sensor to model. // Add sensor to model.
MJAPI mjmSensor* mjm_addSensor(void* model); MJAPI mjmSensor* mjm_addSensor(void* model);
// Add numeric to model.
MJAPI mjmNumeric* mjm_addNumeric(void* model);
// Add text to model.
MJAPI mjmText* mjm_addText(void* model);
// Add tuple to model.
MJAPI mjmTuple* mjm_addTuple(void* model);
// Add keyframe to model.
MJAPI mjmKey* mjm_addKey(void* model);
// Add plugin to model. // Add plugin to model.
MJAPI mjElement mjm_addPlugin(void* model); MJAPI mjElement mjm_addPlugin(void* model);
@@ -720,6 +773,18 @@ MJAPI void mjm_defaultActuator(mjmActuator& actuator);
// Default sensor attributes. // Default sensor attributes.
MJAPI void mjm_defaultSensor(mjmSensor& sensor); MJAPI void mjm_defaultSensor(mjmSensor& sensor);
// Default numeric attributes.
MJAPI void mjm_defaultNumeric(mjmNumeric& numeric);
// Default text attributes.
MJAPI void mjm_defaultText(mjmText& text);
// Default tuple attributes.
MJAPI void mjm_defaultTuple(mjmTuple& tuple);
// Default keyframe attributes.
MJAPI void mjm_defaultKey(mjmKey& key);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif
src/user/user_composite.cc
+3 -3
View File
@@ -698,9 +698,9 @@ bool mjCComposite::MakeCable(mjCModel* model, mjmBody* body, char* error, int er
} }
// add name to model // add name to model
mjCText* pte = model->AddText(); mjmText* pte = mjm_addText(model);
pte->name = "composite_" + prefix; mjm_setString(pte->name, ("composite_" + prefix).c_str());
pte->data = "rope_" + prefix; mjm_setString(pte->data, ("rope_" + prefix).c_str());
// populate uservert if not specified // populate uservert if not specified
if (uservert.empty()) { if (uservert.empty()) {
src/user/user_init.cc
+26
View File
@@ -307,4 +307,30 @@ void mjm_defaultSensor(mjmSensor& sensor) {
// Default numeric attributes.
void mjm_defaultNumeric(mjmNumeric& numeric) {
memset(&numeric, 0, sizeof(mjmNumeric));
}
// Default text attributes.
void mjm_defaultText(mjmText& text) {
memset(&text, 0, sizeof(mjmText));
}
// Default tuple attributes.
void mjm_defaultTuple(mjmTuple& tuple) {
memset(&tuple, 0, sizeof(mjmTuple));
}
// Default keyframe attributes.
void mjm_defaultKey(mjmKey& key) {
memset(&key, 0, sizeof(mjmKey));
}
src/user/user_model.cc
+24 -24
View File
@@ -1069,10 +1069,10 @@ void mjCModel::SetSizes(void) {
for (int i=0; i<nnumeric; i++) nnumericdata += numerics[i]->size; for (int i=0; i<nnumeric; i++) nnumericdata += numerics[i]->size;
// ntextdata // ntextdata
for (int i=0; i<ntext; i++) ntextdata += (int)texts[i]->data.size() + 1; for (int i=0; i<ntext; i++) ntextdata += (int)texts[i]->data_.size() + 1;
// ntupledata // ntupledata
for (int i=0; i<ntuple; i++) ntupledata += (int)tuples[i]->objtype.size(); for (int i=0; i<ntuple; i++) ntupledata += (int)tuples[i]->objtype_.size();
// npluginattr // npluginattr
for (int i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size(); for (int i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size();
@@ -2335,10 +2335,10 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields // set fields
m->numeric_adr[i] = adr; m->numeric_adr[i] = adr;
m->numeric_size[i] = pcu->size; m->numeric_size[i] = pcu->size;
for (int j=0; j<(int)pcu->data.size(); j++) { for (int j=0; j<(int)pcu->data_.size(); j++) {
m->numeric_data[adr+j] = (mjtNum)pcu->data[j]; m->numeric_data[adr+j] = (mjtNum)pcu->data_[j];
} }
for (int j=(int)pcu->data.size(); j<(int)pcu->size; j++) { for (int j=(int)pcu->data_.size(); j<(int)pcu->size; j++) {
m->numeric_data[adr+j] = 0; m->numeric_data[adr+j] = 0;
} }
@@ -2354,8 +2354,8 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields // set fields
m->text_adr[i] = adr; m->text_adr[i] = adr;
m->text_size[i] = (int)pte->data.size()+1; m->text_size[i] = (int)pte->data_.size()+1;
mju_strncpy(m->text_data + adr, pte->data.c_str(), m->ntextdata - adr); mju_strncpy(m->text_data + adr, pte->data_.c_str(), m->ntextdata - adr);
// advance address counter // advance address counter
adr += m->text_size[i]; adr += m->text_size[i];
@@ -2369,11 +2369,11 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields // set fields
m->tuple_adr[i] = adr; m->tuple_adr[i] = adr;
m->tuple_size[i] = (int)ptu->objtype.size(); m->tuple_size[i] = (int)ptu->objtype_.size();
for (int j=0; j<m->tuple_size[i]; j++) { for (int j=0; j<m->tuple_size[i]; j++) {
m->tuple_objtype[adr+j] = (int)ptu->objtype[j]; m->tuple_objtype[adr+j] = (int)ptu->objtype_[j];
m->tuple_objid[adr+j] = ptu->obj[j]->id; m->tuple_objid[adr+j] = ptu->obj[j]->id;
m->tuple_objprm[adr+j] = (mjtNum)ptu->objprm[j]; m->tuple_objprm[adr+j] = (mjtNum)ptu->objprm_[j];
} }
// advance address counter // advance address counter
@@ -2384,14 +2384,14 @@ void mjCModel::CopyObjects(mjModel* m) {
for (int i=0; i<nkey; i++) { for (int i=0; i<nkey; i++) {
// copy data // copy data
m->key_time[i] = (mjtNum)keys[i]->time; m->key_time[i] = (mjtNum)keys[i]->time;
copyvec(m->key_qpos+i*nq, keys[i]->qpos.data(), nq); copyvec(m->key_qpos+i*nq, keys[i]->qpos_.data(), nq);
copyvec(m->key_qvel+i*nv, keys[i]->qvel.data(), nv); copyvec(m->key_qvel+i*nv, keys[i]->qvel_.data(), nv);
if (na) { if (na) {
copyvec(m->key_act+i*na, keys[i]->act.data(), na); copyvec(m->key_act+i*na, keys[i]->act_.data(), na);
} }
if (nmocap) { if (nmocap) {
copyvec(m->key_mpos + i*3*nmocap, keys[i]->mpos.data(), 3*nmocap); copyvec(m->key_mpos + i*3*nmocap, keys[i]->mpos_.data(), 3*nmocap);
copyvec(m->key_mquat + i*4*nmocap, keys[i]->mquat.data(), 4*nmocap); copyvec(m->key_mquat + i*4*nmocap, keys[i]->mquat_.data(), 4*nmocap);
} }
// normalize quaternions in m->key_qpos // normalize quaternions in m->key_qpos
@@ -2406,7 +2406,7 @@ void mjCModel::CopyObjects(mjModel* m) {
mju_normalize4(m->key_mquat+i*4*nmocap+4*j); mju_normalize4(m->key_mquat+i*4*nmocap+4*j);
} }
copyvec(m->key_ctrl+i*nu, keys[i]->ctrl.data(), nu); copyvec(m->key_ctrl+i*nu, keys[i]->ctrl_.data(), nu);
} }
// save qpos0 in user model (to recognize changed key_qpos in write) // save qpos0 in user model (to recognize changed key_qpos in write)
@@ -3518,14 +3518,14 @@ bool mjCModel::CopyBack(const mjModel* m) {
// numeric data // numeric data
for (int i=0; i<nnumeric; i++) { for (int i=0; i<nnumeric; i++) {
for (int j=0; j<m->numeric_size[i]; j++) { for (int j=0; j<m->numeric_size[i]; j++) {
numerics[i]->data[j] = (double)m->numeric_data[m->numeric_adr[i]+j]; numerics[i]->data_[j] = (double)m->numeric_data[m->numeric_adr[i]+j];
} }
} }
// tuple data // tuple data
for (int i=0; i<ntuple; i++) { for (int i=0; i<ntuple; i++) {
for (int j=0; j<m->tuple_size[i]; j++) { for (int j=0; j<m->tuple_size[i]; j++) {
tuples[i]->objprm[j] = (double)m->tuple_objprm[m->tuple_adr[i]+j]; tuples[i]->objprm_[j] = (double)m->tuple_objprm[m->tuple_adr[i]+j];
} }
} }
@@ -3534,17 +3534,17 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjCKey* pk = keys[i]; mjCKey* pk = keys[i];
pk->time = (double)m->key_time[i]; pk->time = (double)m->key_time[i];
copyvec(pk->qpos.data(), m->key_qpos + i*nq, nq); copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq);
copyvec(pk->qvel.data(), m->key_qvel + i*nv, nv); copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv);
if (na) { if (na) {
copyvec(pk->act.data(), m->key_act + i*na, na); copyvec(pk->act_.data(), m->key_act + i*na, na);
} }
if (nmocap) { if (nmocap) {
copyvec(pk->mpos.data(), m->key_mpos + i*3*nmocap, 3*nmocap); copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap);
copyvec(pk->mquat.data(), m->key_mquat + i*4*nmocap, 4*nmocap); copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap);
} }
if (nu) { if (nu) {
copyvec(pk->ctrl.data(), m->key_ctrl + i*nu, nu); copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu);
} }
} }
src/user/user_objects.cc
+201 -63
View File
@@ -4821,27 +4821,54 @@ void mjCSensor::Compile(void) {
// constructor // constructor
mjCNumeric::mjCNumeric(mjCModel* _model) { mjCNumeric::mjCNumeric(mjCModel* _model) {
mjm_defaultNumeric(spec);
// set model pointer // set model pointer
model = _model; model = _model;
// clear variables // clear variables
data.clear(); spec_data_.clear();
size = 0;
// point to local
PointToLocal();
// in case this numeric is not compiled
CopyFromSpec();
}
void mjCNumeric::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.data = (mjDoubleVec)&spec_data_;
spec.info = (mjString)&info;
}
void mjCNumeric::CopyFromSpec() {
*static_cast<mjmNumeric*>(this) = spec;
data_ = spec_data_;
data = (mjDoubleVec)&data_;
} }
// destructor // destructor
mjCNumeric::~mjCNumeric() { mjCNumeric::~mjCNumeric() {
data.clear(); spec_data_.clear();
data_.clear();
} }
// compiler // compiler
void mjCNumeric::Compile(void) { void mjCNumeric::Compile(void) {
CopyFromSpec();
// check for size conflict // check for size conflict
if (size && !data.empty() && size<(int)data.size()) { if (size && !data_.empty() && size<(int)data_.size()) {
throw mjCError(this, throw mjCError(this,
"numeric '%s' (id = %d): specified size smaller than initialization array", "numeric '%s' (id = %d): specified size smaller than initialization array",
name.c_str(), id); name.c_str(), id);
@@ -4849,7 +4876,7 @@ void mjCNumeric::Compile(void) {
// set size if left unspecified // set size if left unspecified
if (!size) { if (!size) {
size = (int)data.size(); size = (int)data_.size();
} }
// size cannot be zero // size cannot be zero
@@ -4864,26 +4891,54 @@ void mjCNumeric::Compile(void) {
// constructor // constructor
mjCText::mjCText(mjCModel* _model) { mjCText::mjCText(mjCModel* _model) {
mjm_defaultText(spec);
// set model pointer // set model pointer
model = _model; model = _model;
// clear variables // clear variables
data.clear(); spec_data_.clear();
// point to local
PointToLocal();
// in case this text is not compiled
CopyFromSpec();
}
void mjCText::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.data = (mjString)&spec_data_;
spec.info = (mjString)&info;
}
void mjCText::CopyFromSpec() {
*static_cast<mjmText*>(this) = spec;
data_ = spec_data_;
data = (mjString)&data_;
} }
// destructor // destructor
mjCText::~mjCText() { mjCText::~mjCText() {
data.clear(); data_.clear();
spec_data_.clear();
} }
// compiler // compiler
void mjCText::Compile(void) { void mjCText::Compile(void) {
CopyFromSpec();
// size cannot be zero // size cannot be zero
if (data.empty()) { if (data_.empty()) {
throw mjCError(this, "text '%s' (id = %d): size cannot be zero", name.c_str(), id); throw mjCError(this, "text '%s' (id = %d): size cannot be zero", name.c_str(), id);
} }
} }
@@ -4894,23 +4949,57 @@ void mjCText::Compile(void) {
// constructor // constructor
mjCTuple::mjCTuple(mjCModel* _model) { mjCTuple::mjCTuple(mjCModel* _model) {
mjm_defaultTuple(spec);
// set model pointer // set model pointer
model = _model; model = _model;
// clear variables // clear variables
objtype.clear(); spec_objtype_.clear();
objname.clear(); spec_objname_.clear();
objprm.clear(); spec_objprm_.clear();
obj.clear(); obj.clear();
// point to local
PointToLocal();
// in case this tuple is not compiled
CopyFromSpec();
}
void mjCTuple::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.objtype = (mjIntVec)&spec_objtype_;
spec.objname = (mjStringVec)&spec_objname_;
spec.objprm = (mjDoubleVec)&spec_objprm_;
spec.info = (mjString)&info;
}
void mjCTuple::CopyFromSpec() {
*static_cast<mjmTuple*>(this) = spec;
objtype_ = spec_objtype_;
objname_ = spec_objname_;
objprm_ = spec_objprm_;
objtype = (mjIntVec)&objtype_;
objname = (mjStringVec)&objname_;
objprm = (mjDoubleVec)&objprm_;
} }
// destructor // destructor
mjCTuple::~mjCTuple() { mjCTuple::~mjCTuple() {
objtype.clear(); objtype_.clear();
objname.clear(); objname_.clear();
objprm.clear(); objprm_.clear();
spec_objtype_.clear();
spec_objname_.clear();
spec_objprm_.clear();
obj.clear(); obj.clear();
} }
@@ -4918,30 +5007,32 @@ mjCTuple::~mjCTuple() {
// compiler // compiler
void mjCTuple::Compile(void) { void mjCTuple::Compile(void) {
CopyFromSpec();
// check for empty tuple // check for empty tuple
if (objtype.empty()) { if (objtype_.empty()) {
throw mjCError(this, "tuple '%s' (id = %d) is empty", name.c_str(), id); throw mjCError(this, "tuple '%s' (id = %d) is empty", name.c_str(), id);
} }
// check for size conflict // check for size conflict
if (objtype.size()!=objname.size() || objtype.size()!=objprm.size()) { if (objtype_.size()!=objname_.size() || objtype_.size()!=objprm_.size()) {
throw mjCError(this, throw mjCError(this,
"tuple '%s' (id = %d) has object arrays with different sizes", name.c_str(), id); "tuple '%s' (id = %d) has object arrays with different sizes", name.c_str(), id);
} }
// resize objid to correct size // resize objid to correct size
obj.resize(objtype.size()); obj.resize(objtype_.size());
// find objects, fill in ids // find objects, fill in ids
for (int i=0; i<objtype.size(); i++) { for (int i=0; i<objtype_.size(); i++) {
// find object by type and name // find object by type and name
mjCBase* res = model->FindObject(objtype[i], objname[i]); mjCBase* res = model->FindObject(objtype_[i], objname_[i]);
if (!res) { if (!res) {
throw mjCError(this, "unrecognized object '%s' in tuple %d", objname[i].c_str(), id); throw mjCError(this, "unrecognized object '%s' in tuple %d", objname_[i].c_str(), id);
} }
// if geom mark it as non visual // if geom mark it as non visual
if (objtype[i] == mjOBJ_GEOM) { if (objtype_[i] == mjOBJ_GEOM) {
((mjCGeom*)res)->SetNotVisual(); ((mjCGeom*)res)->SetNotVisual();
} }
@@ -4956,107 +5047,154 @@ void mjCTuple::Compile(void) {
// constructor // constructor
mjCKey::mjCKey(mjCModel* _model) { mjCKey::mjCKey(mjCModel* _model) {
mjm_defaultKey(spec);
// set model pointer // set model pointer
model = _model; model = _model;
// clear variables // clear variables
time = 0; spec_qpos_.clear();
qpos.clear(); spec_qvel_.clear();
qvel.clear(); spec_act_.clear();
act.clear(); spec_mpos_.clear();
mpos.clear(); spec_mquat_.clear();
mquat.clear(); spec_ctrl_.clear();
ctrl.clear();
// point to local
PointToLocal();
// in case this keyframe is not compiled
CopyFromSpec();
}
void mjCKey::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.qpos = (mjDoubleVec)&spec_qpos_;
spec.qvel = (mjDoubleVec)&spec_qvel_;
spec.act = (mjDoubleVec)&spec_act_;
spec.mpos = (mjDoubleVec)&spec_mpos_;
spec.mquat = (mjDoubleVec)&spec_mquat_;
spec.ctrl = (mjDoubleVec)&spec_ctrl_;
spec.info = (mjString)&info;
}
void mjCKey::CopyFromSpec() {
*static_cast<mjmKey*>(this) = spec;
qpos_ = spec_qpos_;
qvel_ = spec_qvel_;
act_ = spec_act_;
mpos_ = spec_mpos_;
mquat_ = spec_mquat_;
ctrl_ = spec_ctrl_;
qpos = (mjDoubleVec)&qpos_;
qvel = (mjDoubleVec)&qvel_;
act = (mjDoubleVec)&act_;
mpos = (mjDoubleVec)&mpos_;
mquat = (mjDoubleVec)&mquat_;
ctrl = (mjDoubleVec)&ctrl_;
} }
// destructor // destructor
mjCKey::~mjCKey() { mjCKey::~mjCKey() {
qpos.clear(); qpos_.clear();
qvel.clear(); qvel_.clear();
act.clear(); act_.clear();
mpos.clear(); mpos_.clear();
mquat.clear(); mquat_.clear();
ctrl.clear(); ctrl_.clear();
spec_qpos_.clear();
spec_qvel_.clear();
spec_act_.clear();
spec_mpos_.clear();
spec_mquat_.clear();
spec_ctrl_.clear();
} }
// compiler // compiler
void mjCKey::Compile(const mjModel* m) { void mjCKey::Compile(const mjModel* m) {
CopyFromSpec();
// qpos: allocate or check size // qpos: allocate or check size
if (qpos.empty()) { if (qpos_.empty()) {
qpos.resize(m->nq); qpos_.resize(m->nq);
for (int i=0; i<m->nq; i++) { for (int i=0; i<m->nq; i++) {
qpos[i] = (double)m->qpos0[i]; qpos_[i] = (double)m->qpos0[i];
} }
} else if (qpos.size()!=m->nq) { } else if (qpos_.size()!=m->nq) {
throw mjCError(this, "key %d: invalid qpos size, expected length %d", nullptr, id, m->nq); throw mjCError(this, "key %d: invalid qpos size, expected length %d", nullptr, id, m->nq);
} }
// qvel: allocate or check size // qvel: allocate or check size
if (qvel.empty()) { if (qvel_.empty()) {
qvel.resize(m->nv); qvel_.resize(m->nv);
for (int i=0; i<m->nv; i++) { for (int i=0; i<m->nv; i++) {
qvel[i] = 0; qvel_[i] = 0;
} }
} else if (qvel.size()!=m->nv) { } else if (qvel_.size()!=m->nv) {
throw mjCError(this, "key %d: invalid qvel size, expected length %d", nullptr, id, m->nv); throw mjCError(this, "key %d: invalid qvel size, expected length %d", nullptr, id, m->nv);
} }
// act: allocate or check size // act: allocate or check size
if (act.empty()) { if (act_.empty()) {
act.resize(m->na); act_.resize(m->na);
for (int i=0; i<m->na; i++) { for (int i=0; i<m->na; i++) {
act[i] = 0; act_[i] = 0;
} }
} else if (act.size()!=m->na) { } else if (act_.size()!=m->na) {
throw mjCError(this, "key %d: invalid act size, expected length %d", nullptr, id, m->na); throw mjCError(this, "key %d: invalid act size, expected length %d", nullptr, id, m->na);
} }
// mpos: allocate or check size // mpos: allocate or check size
if (mpos.empty()) { if (mpos_.empty()) {
mpos.resize(3*m->nmocap); mpos_.resize(3*m->nmocap);
if (m->nmocap) { if (m->nmocap) {
for (int i=0; i<m->nbody; i++) { for (int i=0; i<m->nbody; i++) {
if (m->body_mocapid[i]>=0) { if (m->body_mocapid[i]>=0) {
int mocapid = m->body_mocapid[i]; int mocapid = m->body_mocapid[i];
mpos[3*mocapid] = m->body_pos[3*i]; mpos_[3*mocapid] = m->body_pos[3*i];
mpos[3*mocapid+1] = m->body_pos[3*i+1]; mpos_[3*mocapid+1] = m->body_pos[3*i+1];
mpos[3*mocapid+2] = m->body_pos[3*i+2]; mpos_[3*mocapid+2] = m->body_pos[3*i+2];
} }
} }
} }
} else if (mpos.size()!=3*m->nmocap) { } else if (mpos_.size()!=3*m->nmocap) {
throw mjCError(this, "key %d: invalid mpos size, expected length %d", nullptr, id, 3*m->nmocap); throw mjCError(this, "key %d: invalid mpos size, expected length %d", nullptr, id, 3*m->nmocap);
} }
// mquat: allocate or check size // mquat: allocate or check size
if (mquat.empty()) { if (mquat_.empty()) {
mquat.resize(4*m->nmocap); mquat_.resize(4*m->nmocap);
if (m->nmocap) { if (m->nmocap) {
for (int i=0; i<m->nbody; i++) { for (int i=0; i<m->nbody; i++) {
if (m->body_mocapid[i]>=0) { if (m->body_mocapid[i]>=0) {
int mocapid = m->body_mocapid[i]; int mocapid = m->body_mocapid[i];
mquat[4*mocapid] = m->body_quat[4*i]; mquat_[4*mocapid] = m->body_quat[4*i];
mquat[4*mocapid+1] = m->body_quat[4*i+1]; mquat_[4*mocapid+1] = m->body_quat[4*i+1];
mquat[4*mocapid+2] = m->body_quat[4*i+2]; mquat_[4*mocapid+2] = m->body_quat[4*i+2];
mquat[4*mocapid+3] = m->body_quat[4*i+3]; mquat_[4*mocapid+3] = m->body_quat[4*i+3];
} }
} }
} }
} else if (mquat.size()!=4*m->nmocap) { } else if (mquat_.size()!=4*m->nmocap) {
throw mjCError(this, "key %d: invalid mquat size, expected length %d", nullptr, id, 4*m->nmocap); throw mjCError(this, "key %d: invalid mquat size, expected length %d", nullptr, id, 4*m->nmocap);
} }
// ctrl: allocate or check size // ctrl: allocate or check size
if (ctrl.empty()) { if (ctrl_.empty()) {
ctrl.resize(m->nu); ctrl_.resize(m->nu);
for (int i=0; i<m->nu; i++) { for (int i=0; i<m->nu; i++) {
ctrl[i] = 0; ctrl_[i] = 0;
} }
} else if (ctrl.size()!=m->nu) { } else if (ctrl_.size()!=m->nu) {
throw mjCError(this, "key %d: invalid ctrl size, expected length %d", nullptr, id, m->nu); throw mjCError(this, "key %d: invalid ctrl size, expected length %d", nullptr, id, m->nu);
} }
} }
src/user/user_objects.h
+58 -20
View File
@@ -1235,18 +1235,25 @@ class mjCSensor : public mjCBase, private mjmSensor {
//------------------------- class mjCNumeric ------------------------------------------------------- //------------------------- class mjCNumeric -------------------------------------------------------
// Describes a custom data field // Describes a custom data field
class mjCNumeric : public mjCBase { class mjCNumeric : public mjCBase, private mjmNumeric {
friend class mjCModel; friend class mjCModel;
friend class mjXWriter;
public: public:
// variables set by user mjmNumeric spec;
std::vector<double> data; // initialization data using mjCBase::name;
int size; // array size, can be bigger than data.size() using mjCBase::info;
void PointToLocal();
void CopyFromSpec();
private: private:
mjCNumeric(mjCModel*); // constructor mjCNumeric(mjCModel*); // constructor
~mjCNumeric(); // destructor ~mjCNumeric(); // destructor
void Compile(void); // compiler void Compile(void); // compiler
std::vector<double> data_;
std::vector<double> spec_data_;
}; };
@@ -1254,17 +1261,25 @@ class mjCNumeric : public mjCBase {
//------------------------- class mjCText ---------------------------------------------------------- //------------------------- class mjCText ----------------------------------------------------------
// Describes a custom text field // Describes a custom text field
class mjCText : public mjCBase { class mjCText : public mjCBase, private mjmText {
friend class mjCModel; friend class mjCModel;
friend class mjXWriter;
public: public:
// variables set by user mjmText spec;
std::string data; // string using mjCBase::name;
using mjCBase::info;
void PointToLocal();
void CopyFromSpec();
private: private:
mjCText(mjCModel*); // constructor mjCText(mjCModel*); // constructor
~mjCText(); // destructor ~mjCText(); // destructor
void Compile(void); // compiler void Compile(void); // compiler
std::string data_;
std::string spec_data_;
}; };
@@ -1272,14 +1287,17 @@ class mjCText : public mjCBase {
//------------------------- class mjCTuple --------------------------------------------------------- //------------------------- class mjCTuple ---------------------------------------------------------
// Describes a custom tuple field // Describes a custom tuple field
class mjCTuple : public mjCBase { class mjCTuple : public mjCBase, private mjmTuple {
friend class mjCModel; friend class mjCModel;
friend class mjXWriter;
public: public:
// variables set by user mjmTuple spec;
std::vector<mjtObj> objtype; // object types using mjCBase::name;
std::vector<std::string> objname; // object names using mjCBase::info;
std::vector<double> objprm; // object parameters
void PointToLocal();
void CopyFromSpec();
private: private:
mjCTuple(mjCModel*); // constructor mjCTuple(mjCModel*); // constructor
@@ -1287,6 +1305,14 @@ class mjCTuple : public mjCBase {
void Compile(void); // compiler void Compile(void); // compiler
std::vector<mjCBase*> obj; // object pointers std::vector<mjCBase*> obj; // object pointers
// variable-size data
std::vector<mjtObj> objtype_;
std::vector<std::string> objname_;
std::vector<double> objprm_;
std::vector<mjtObj> spec_objtype_;
std::vector<std::string> spec_objname_;
std::vector<double> spec_objprm_;
}; };
@@ -1294,23 +1320,35 @@ class mjCTuple : public mjCBase {
//------------------------- class mjCKey ----------------------------------------------------------- //------------------------- class mjCKey -----------------------------------------------------------
// Describes a keyframe // Describes a keyframe
class mjCKey : public mjCBase { class mjCKey : public mjCBase, private mjmKey {
friend class mjCModel; friend class mjCModel;
friend class mjXWriter; friend class mjXWriter;
public: public:
double time; // time mjmKey spec;
std::vector<double> qpos; // qpos using mjCBase::name;
std::vector<double> qvel; // qvel using mjCBase::info;
std::vector<double> act; // act
std::vector<double> mpos; // mocap pos void PointToLocal();
std::vector<double> mquat; // mocap quat void CopyFromSpec();
std::vector<double> ctrl; // ctrl
private: private:
mjCKey(mjCModel*); // constructor mjCKey(mjCModel*); // constructor
~mjCKey(); // destructor ~mjCKey(); // destructor
void Compile(const mjModel* m); // compiler void Compile(const mjModel* m); // compiler
std::vector<double> qpos_;
std::vector<double> qvel_;
std::vector<double> act_;
std::vector<double> mpos_;
std::vector<double> mquat_;
std::vector<double> ctrl_;
std::vector<double> spec_qpos_;
std::vector<double> spec_qvel_;
std::vector<double> spec_act_;
std::vector<double> spec_mpos_;
std::vector<double> spec_mquat_;
std::vector<double> spec_ctrl_;
}; };
src/xml/xml_native_reader.cc
+46 -31
View File
@@ -2781,15 +2781,20 @@ void mjXReader::Custom(XMLElement* section) {
while (elem) { while (elem) {
// get sub-element name // get sub-element name
name = elem->Value(); name = elem->Value();
string elname;
// numeric // numeric
if (name=="numeric") { if (name=="numeric") {
// create custom // create custom
mjCNumeric* pnum = model->AddNumeric(); mjmNumeric* pnum = mjm_addNumeric(model);
GetXMLPos(elem, pnum);
// write error info
mjm_setString(pnum->info,
std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str());
// read attributes // read attributes
ReadAttrTxt(elem, "name", pnum->name, true); ReadAttrTxt(elem, "name", elname, true);
mjm_setString(pnum->name, elname.c_str());
if (ReadAttrInt(elem, "size", &pnum->size)) { if (ReadAttrInt(elem, "size", &pnum->size)) {
int sz = pnum->size < 500 ? pnum->size : 500; int sz = pnum->size < 500 ? pnum->size : 500;
for (int i=0; i<sz; i++) { for (int i=0; i<sz; i++) {
@@ -2807,39 +2812,49 @@ void mjXReader::Custom(XMLElement* section) {
} }
// copy data // copy data
for (int i=0; i<pnum->size; i++) { mjm_setDouble(pnum->data, data, pnum->size);
pnum->data.push_back(data[i]);
}
} }
// text // text
else if (name=="text") { else if (name=="text") {
// create custom // create custom
mjCText* pte = model->AddText(); mjmText* pte = mjm_addText(model);
GetXMLPos(elem, pte);
// write error info
mjm_setString(pte->info,
std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str());
// read attributes // read attributes
ReadAttrTxt(elem, "name", pte->name, true); ReadAttrTxt(elem, "name", elname, true);
mjm_setString(pte->name, elname.c_str());
ReadAttrTxt(elem, "data", text, true); ReadAttrTxt(elem, "data", text, true);
if (text.empty()) { if (text.empty()) {
throw mjXError(elem, "text field cannot be empty"); throw mjXError(elem, "text field cannot be empty");
} }
// copy data // copy data
pte->data = text; mjm_setString(pte->data, text.c_str());
} }
// tuple // tuple
else if (name=="tuple") { else if (name=="tuple") {
// create custom // create custom
mjCTuple* ptu = model->AddTuple(); mjmTuple* ptu = mjm_addTuple(model);
GetXMLPos(elem, ptu);
// write error info
mjm_setString(ptu->info,
std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str());
// read attributes // read attributes
ReadAttrTxt(elem, "name", ptu->name, true); ReadAttrTxt(elem, "name", elname, true);
mjm_setString(ptu->name, elname.c_str());
// read objects and add // read objects and add
XMLElement* obj = FirstChildElement(elem); XMLElement* obj = FirstChildElement(elem);
std::vector<int> objtype;
std::string objname = "";
std::vector<double> objprm;
while (obj) { while (obj) {
// get sub-element name // get sub-element name
name = obj->Value(); name = obj->Value();
@@ -2852,21 +2867,25 @@ void mjXReader::Custom(XMLElement* section) {
if (otype==mjOBJ_UNKNOWN) { if (otype==mjOBJ_UNKNOWN) {
throw mjXError(obj, "unknown object type"); throw mjXError(obj, "unknown object type");
} }
ptu->objtype.push_back(otype); objtype.push_back(otype);
// read name and assign // read name and assign
ReadAttrTxt(obj, "objname", text, true); ReadAttrTxt(obj, "objname", text, true);
ptu->objname.push_back(text); objname += text + " ";
// read parameter and assign // read parameter and assign
double oprm = 0; double oprm = 0;
ReadAttr(obj, "prm", 1, &oprm, text); ReadAttr(obj, "prm", 1, &oprm, text);
ptu->objprm.push_back(oprm); objprm.push_back(oprm);
} }
// advance to next object // advance to next object
obj = NextSiblingElement(obj); obj = NextSiblingElement(obj);
} }
mjm_setInt(ptu->objtype, objtype.data(), objtype.size());
mjm_setStringVec(ptu->objname, objname.c_str());
mjm_setDouble(ptu->objprm, objprm.data(), objprm.size());
} }
// advance to next element // advance to next element
@@ -3875,7 +3894,6 @@ void mjXReader::Sensor(XMLElement* section) {
// keyframe section parser // keyframe section parser
void mjXReader::Keyframe(XMLElement* section) { void mjXReader::Keyframe(XMLElement* section) {
string text;
XMLElement* elem; XMLElement* elem;
int n; int n;
double data[1000]; double data[1000];
@@ -3883,53 +3901,50 @@ void mjXReader::Keyframe(XMLElement* section) {
// iterate over child elements // iterate over child elements
elem = FirstChildElement(section); elem = FirstChildElement(section);
while (elem) { while (elem) {
string text, name = "";
// add keyframe // add keyframe
mjCKey* pk = model->AddKey(); mjmKey* pk = mjm_addKey(model);
// read name, time // read name, time
ReadAttrTxt(elem, "name", pk->name); ReadAttrTxt(elem, "name", name);
mjm_setString(pk->name, name.c_str());
ReadAttr(elem, "time", 1, &pk->time, text); ReadAttr(elem, "time", 1, &pk->time, text);
// read qpos // read qpos
n = ReadAttr(elem, "qpos", 1000, data, text, false, false); n = ReadAttr(elem, "qpos", 1000, data, text, false, false);
if (n) { if (n) {
pk->qpos.resize(n); mjm_setDouble(pk->qpos, data, n);
mjuu_copyvec(pk->qpos.data(), data, n);
} }
// read qvel // read qvel
n = ReadAttr(elem, "qvel", 1000, data, text, false, false); n = ReadAttr(elem, "qvel", 1000, data, text, false, false);
if (n) { if (n) {
pk->qvel.resize(n); mjm_setDouble(pk->qvel, data, n);
mjuu_copyvec(pk->qvel.data(), data, n);
} }
// read act // read act
n = ReadAttr(elem, "act", 1000, data, text, false, false); n = ReadAttr(elem, "act", 1000, data, text, false, false);
if (n) { if (n) {
pk->act.resize(n); mjm_setDouble(pk->act, data, n);
mjuu_copyvec(pk->act.data(), data, n);
} }
// read mpos // read mpos
n = ReadAttr(elem, "mpos", 1000, data, text, false, false); n = ReadAttr(elem, "mpos", 1000, data, text, false, false);
if (n) { if (n) {
pk->mpos.resize(n); mjm_setDouble(pk->mpos, data, n);
mjuu_copyvec(pk->mpos.data(), data, n);
} }
// read mquat // read mquat
n = ReadAttr(elem, "mquat", 1000, data, text, false, false); n = ReadAttr(elem, "mquat", 1000, data, text, false, false);
if (n) { if (n) {
pk->mquat.resize(n); mjm_setDouble(pk->mquat, data, n);
mjuu_copyvec(pk->mquat.data(), data, n);
} }
// read ctrl // read ctrl
n = ReadAttr(elem, "ctrl", 1000, data, text, false, false); n = ReadAttr(elem, "ctrl", 1000, data, text, false, false);
if (n) { if (n) {
pk->ctrl.resize(n); mjm_setDouble(pk->ctrl, data, n);
mjuu_copyvec(pk->ctrl.data(), data, n);
} }
// advance to next element // advance to next element
src/xml/xml_native_writer.cc
+23 -23
View File
@@ -1283,7 +1283,7 @@ void mjXWriter::Custom(XMLElement* root) {
elem = InsertEnd(section, "numeric"); elem = InsertEnd(section, "numeric");
WriteAttrTxt(elem, "name", ptr->name); WriteAttrTxt(elem, "name", ptr->name);
WriteAttrInt(elem, "size", ptr->size); WriteAttrInt(elem, "size", ptr->size);
WriteAttr(elem, "data", ptr->size, ptr->data.data()); WriteAttr(elem, "data", ptr->size, ptr->data_.data());
} }
// write all texts // write all texts
@@ -1291,7 +1291,7 @@ void mjXWriter::Custom(XMLElement* root) {
mjCText* ptr = (mjCText*)model->GetObject(mjOBJ_TEXT, i); mjCText* ptr = (mjCText*)model->GetObject(mjOBJ_TEXT, i);
elem = InsertEnd(section, "text"); elem = InsertEnd(section, "text");
WriteAttrTxt(elem, "name", ptr->name); WriteAttrTxt(elem, "name", ptr->name);
WriteAttrTxt(elem, "data", ptr->data.c_str()); WriteAttrTxt(elem, "data", ptr->data_.c_str());
} }
// write all tuples // write all tuples
@@ -1301,11 +1301,11 @@ void mjXWriter::Custom(XMLElement* root) {
WriteAttrTxt(elem, "name", ptr->name); WriteAttrTxt(elem, "name", ptr->name);
// write objects in tuple // write objects in tuple
for (int j=0; j<(int)ptr->objtype.size(); j++) { for (int j=0; j<(int)ptr->objtype_.size(); j++) {
XMLElement* obj = InsertEnd(elem, "element"); XMLElement* obj = InsertEnd(elem, "element");
WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype[j])); WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype_[j]));
WriteAttrTxt(obj, "objname", ptr->objname[j].c_str()); WriteAttrTxt(obj, "objname", ptr->objname_[j].c_str());
double oprm = ptr->objprm[j]; double oprm = ptr->objprm_[j];
if (oprm!=0) { if (oprm!=0) {
WriteAttr(obj, "prm", 1, &oprm); WriteAttr(obj, "prm", 1, &oprm);
} }
@@ -1964,8 +1964,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check qpos and write // check qpos and write
for (int j=0; j<model->nq; j++) { for (int j=0; j<model->nq; j++) {
if (pk->qpos[j]!=model->qpos0[j]) { if (pk->qpos_[j]!=model->qpos0[j]) {
WriteAttr(elem, "qpos", model->nq, pk->qpos.data()); WriteAttr(elem, "qpos", model->nq, pk->qpos_.data());
change = true; change = true;
break; break;
} }
@@ -1973,8 +1973,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check qvel and write // check qvel and write
for (int j=0; j<model->nv; j++) { for (int j=0; j<model->nv; j++) {
if (pk->qvel[j]!=0) { if (pk->qvel_[j]!=0) {
WriteAttr(elem, "qvel", model->nv, pk->qvel.data()); WriteAttr(elem, "qvel", model->nv, pk->qvel_.data());
change = true; change = true;
break; break;
} }
@@ -1982,8 +1982,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check act and write // check act and write
for (int j=0; j<model->na; j++) { for (int j=0; j<model->na; j++) {
if (pk->act[j]!=0) { if (pk->act_[j]!=0) {
WriteAttr(elem, "act", model->na, pk->act.data()); WriteAttr(elem, "act", model->na, pk->act_.data());
change = true; change = true;
break; break;
} }
@@ -1995,10 +1995,10 @@ void mjXWriter::Keyframe(XMLElement* root) {
if (model->bodies[j]->mocap) { if (model->bodies[j]->mocap) {
mjCBody* pb = model->bodies[j]; mjCBody* pb = model->bodies[j];
int id = pb->mocapid; int id = pb->mocapid;
if (pb->pos[0] != pk->mpos[3*id] || if (pb->pos[0] != pk->mpos_[3*id] ||
pb->pos[1] != pk->mpos[3*id+1] || pb->pos[1] != pk->mpos_[3*id+1] ||
pb->pos[2] != pk->mpos[3*id+2]) { pb->pos[2] != pk->mpos_[3*id+2]) {
WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos.data()); WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos_.data());
change = true; change = true;
break; break;
} }
@@ -2012,11 +2012,11 @@ void mjXWriter::Keyframe(XMLElement* root) {
if (model->bodies[j]->mocap) { if (model->bodies[j]->mocap) {
mjCBody* pb = model->bodies[j]; mjCBody* pb = model->bodies[j];
int id = pb->mocapid; int id = pb->mocapid;
if (pb->quat[0] != pk->mquat[4*id] || if (pb->quat[0] != pk->mquat_[4*id] ||
pb->quat[1] != pk->mquat[4*id+1] || pb->quat[1] != pk->mquat_[4*id+1] ||
pb->quat[2] != pk->mquat[4*id+2] || pb->quat[2] != pk->mquat_[4*id+2] ||
pb->quat[3] != pk->mquat[4*id+3]) { pb->quat[3] != pk->mquat_[4*id+3]) {
WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat.data()); WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat_.data());
change = true; change = true;
break; break;
} }
@@ -2026,8 +2026,8 @@ void mjXWriter::Keyframe(XMLElement* root) {
// check ctrl and write // check ctrl and write
for (int j=0; j<model->nu; j++) { for (int j=0; j<model->nu; j++) {
if (pk->ctrl[j]!=0) { if (pk->ctrl_[j]!=0) {
WriteAttr(elem, "ctrl", model->nu, pk->ctrl.data()); WriteAttr(elem, "ctrl", model->nu, pk->ctrl_.data());
change = true; change = true;
break; break;
} }