diff --git a/src/user/user_api.cc b/src/user/user_api.cc index ed4b2ef7..8ff78504 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -251,6 +251,42 @@ mjmSensor* mjm_addSensor(void* model) { +// add numeric to model +mjmNumeric* mjm_addNumeric(void* model) { + mjCModel* modelC = static_cast(model); + mjCNumeric* numeric = modelC->AddNumeric(); + return &numeric->spec; +} + + + +// add text to model +mjmText* mjm_addText(void* model) { + mjCModel* modelC = static_cast(model); + mjCText* text = modelC->AddText(); + return &text->spec; +} + + + +// add tuple to model +mjmTuple* mjm_addTuple(void* model) { + mjCModel* modelC = static_cast(model); + mjCTuple* tuple = modelC->AddTuple(); + return &tuple->spec; +} + + + +// add keyframe to model +mjmKey* mjm_addKey(void* model) { + mjCModel* modelC = static_cast(model); + mjCKey* key = modelC->AddKey(); + return &key->spec; +} + + + // Add plugin to model. mjElement mjm_addPlugin(void* model) { mjCModel* modelC = static_cast(model); diff --git a/src/user/user_api.h b/src/user/user_api.h index 7b5cafe1..9db90f23 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -541,6 +541,47 @@ typedef struct _mjmSensor { // sensor specfication } 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 ------------------------------------------------- // Create model. @@ -615,6 +656,18 @@ MJAPI mjmActuator* mjm_addActuator(void* model, void* defspec); // Add sensor to 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. MJAPI mjElement mjm_addPlugin(void* model); @@ -720,6 +773,18 @@ MJAPI void mjm_defaultActuator(mjmActuator& actuator); // Default sensor attributes. 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 } #endif diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 7ab810d3..c008a7b8 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -698,9 +698,9 @@ bool mjCComposite::MakeCable(mjCModel* model, mjmBody* body, char* error, int er } // add name to model - mjCText* pte = model->AddText(); - pte->name = "composite_" + prefix; - pte->data = "rope_" + prefix; + mjmText* pte = mjm_addText(model); + mjm_setString(pte->name, ("composite_" + prefix).c_str()); + mjm_setString(pte->data, ("rope_" + prefix).c_str()); // populate uservert if not specified if (uservert.empty()) { diff --git a/src/user/user_init.cc b/src/user/user_init.cc index b900d089..d701b0ea 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -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)); +} + diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 7201240c..860d9a0e 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1069,10 +1069,10 @@ void mjCModel::SetSizes(void) { for (int i=0; isize; // ntextdata - for (int i=0; idata.size() + 1; + for (int i=0; idata_.size() + 1; // ntupledata - for (int i=0; iobjtype.size(); + for (int i=0; iobjtype_.size(); // npluginattr for (int i=0; iflattened_attributes.size(); @@ -2335,10 +2335,10 @@ void mjCModel::CopyObjects(mjModel* m) { // set fields m->numeric_adr[i] = adr; m->numeric_size[i] = pcu->size; - for (int j=0; j<(int)pcu->data.size(); j++) { - m->numeric_data[adr+j] = (mjtNum)pcu->data[j]; + for (int j=0; j<(int)pcu->data_.size(); 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; } @@ -2354,8 +2354,8 @@ void mjCModel::CopyObjects(mjModel* m) { // set fields m->text_adr[i] = adr; - m->text_size[i] = (int)pte->data.size()+1; - mju_strncpy(m->text_data + adr, pte->data.c_str(), m->ntextdata - adr); + m->text_size[i] = (int)pte->data_.size()+1; + mju_strncpy(m->text_data + adr, pte->data_.c_str(), m->ntextdata - adr); // advance address counter adr += m->text_size[i]; @@ -2369,11 +2369,11 @@ void mjCModel::CopyObjects(mjModel* m) { // set fields 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; jtuple_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_objprm[adr+j] = (mjtNum)ptu->objprm[j]; + m->tuple_objprm[adr+j] = (mjtNum)ptu->objprm_[j]; } // advance address counter @@ -2384,14 +2384,14 @@ void mjCModel::CopyObjects(mjModel* m) { for (int i=0; ikey_time[i] = (mjtNum)keys[i]->time; - 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_qpos+i*nq, keys[i]->qpos_.data(), nq); + copyvec(m->key_qvel+i*nv, keys[i]->qvel_.data(), nv); 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) { - 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_mpos + i*3*nmocap, keys[i]->mpos_.data(), 3*nmocap); + copyvec(m->key_mquat + i*4*nmocap, keys[i]->mquat_.data(), 4*nmocap); } // 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); } - 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) @@ -3518,14 +3518,14 @@ bool mjCModel::CopyBack(const mjModel* m) { // numeric data for (int i=0; inumeric_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 for (int i=0; ituple_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]; pk->time = (double)m->key_time[i]; - copyvec(pk->qpos.data(), m->key_qpos + i*nq, nq); - copyvec(pk->qvel.data(), m->key_qvel + i*nv, nv); + copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq); + copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv); if (na) { - copyvec(pk->act.data(), m->key_act + i*na, na); + copyvec(pk->act_.data(), m->key_act + i*na, na); } if (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->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap); + copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap); } if (nu) { - copyvec(pk->ctrl.data(), m->key_ctrl + i*nu, nu); + copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 9c61e483..77a92551 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -4821,27 +4821,54 @@ void mjCSensor::Compile(void) { // constructor mjCNumeric::mjCNumeric(mjCModel* _model) { + mjm_defaultNumeric(spec); + // set model pointer model = _model; // clear variables - data.clear(); - size = 0; + spec_data_.clear(); + + // 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(this) = spec; + data_ = spec_data_; + data = (mjDoubleVec)&data_; } // destructor mjCNumeric::~mjCNumeric() { - data.clear(); + spec_data_.clear(); + data_.clear(); } // compiler void mjCNumeric::Compile(void) { + CopyFromSpec(); + // check for size conflict - if (size && !data.empty() && size<(int)data.size()) { + if (size && !data_.empty() && size<(int)data_.size()) { throw mjCError(this, "numeric '%s' (id = %d): specified size smaller than initialization array", name.c_str(), id); @@ -4849,7 +4876,7 @@ void mjCNumeric::Compile(void) { // set size if left unspecified if (!size) { - size = (int)data.size(); + size = (int)data_.size(); } // size cannot be zero @@ -4864,26 +4891,54 @@ void mjCNumeric::Compile(void) { // constructor mjCText::mjCText(mjCModel* _model) { + mjm_defaultText(spec); + // set model pointer model = _model; // 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(this) = spec; + data_ = spec_data_; + data = (mjString)&data_; } // destructor mjCText::~mjCText() { - data.clear(); + data_.clear(); + spec_data_.clear(); } // compiler void mjCText::Compile(void) { + CopyFromSpec(); + // 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); } } @@ -4894,23 +4949,57 @@ void mjCText::Compile(void) { // constructor mjCTuple::mjCTuple(mjCModel* _model) { + mjm_defaultTuple(spec); + // set model pointer model = _model; // clear variables - objtype.clear(); - objname.clear(); - objprm.clear(); + spec_objtype_.clear(); + spec_objname_.clear(); + spec_objprm_.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(this) = spec; + objtype_ = spec_objtype_; + objname_ = spec_objname_; + objprm_ = spec_objprm_; + objtype = (mjIntVec)&objtype_; + objname = (mjStringVec)&objname_; + objprm = (mjDoubleVec)&objprm_; } // destructor mjCTuple::~mjCTuple() { - objtype.clear(); - objname.clear(); - objprm.clear(); + objtype_.clear(); + objname_.clear(); + objprm_.clear(); + spec_objtype_.clear(); + spec_objname_.clear(); + spec_objprm_.clear(); obj.clear(); } @@ -4918,30 +5007,32 @@ mjCTuple::~mjCTuple() { // compiler void mjCTuple::Compile(void) { + CopyFromSpec(); + // check for empty tuple - if (objtype.empty()) { + if (objtype_.empty()) { throw mjCError(this, "tuple '%s' (id = %d) is empty", name.c_str(), id); } // 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, "tuple '%s' (id = %d) has object arrays with different sizes", name.c_str(), id); } // resize objid to correct size - obj.resize(objtype.size()); + obj.resize(objtype_.size()); // find objects, fill in ids - for (int i=0; iFindObject(objtype[i], objname[i]); + mjCBase* res = model->FindObject(objtype_[i], objname_[i]); 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 (objtype[i] == mjOBJ_GEOM) { + if (objtype_[i] == mjOBJ_GEOM) { ((mjCGeom*)res)->SetNotVisual(); } @@ -4956,107 +5047,154 @@ void mjCTuple::Compile(void) { // constructor mjCKey::mjCKey(mjCModel* _model) { + mjm_defaultKey(spec); + // set model pointer model = _model; // clear variables - time = 0; - qpos.clear(); - qvel.clear(); - act.clear(); - mpos.clear(); - mquat.clear(); - ctrl.clear(); + spec_qpos_.clear(); + spec_qvel_.clear(); + spec_act_.clear(); + spec_mpos_.clear(); + spec_mquat_.clear(); + spec_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(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 mjCKey::~mjCKey() { - qpos.clear(); - qvel.clear(); - act.clear(); - mpos.clear(); - mquat.clear(); - ctrl.clear(); + qpos_.clear(); + qvel_.clear(); + act_.clear(); + mpos_.clear(); + mquat_.clear(); + ctrl_.clear(); + spec_qpos_.clear(); + spec_qvel_.clear(); + spec_act_.clear(); + spec_mpos_.clear(); + spec_mquat_.clear(); + spec_ctrl_.clear(); } // compiler void mjCKey::Compile(const mjModel* m) { + CopyFromSpec(); + // qpos: allocate or check size - if (qpos.empty()) { - qpos.resize(m->nq); + if (qpos_.empty()) { + qpos_.resize(m->nq); for (int i=0; inq; 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); } // qvel: allocate or check size - if (qvel.empty()) { - qvel.resize(m->nv); + if (qvel_.empty()) { + qvel_.resize(m->nv); for (int i=0; inv; 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); } // act: allocate or check size - if (act.empty()) { - act.resize(m->na); + if (act_.empty()) { + act_.resize(m->na); for (int i=0; ina; 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); } // mpos: allocate or check size - if (mpos.empty()) { - mpos.resize(3*m->nmocap); + if (mpos_.empty()) { + mpos_.resize(3*m->nmocap); if (m->nmocap) { for (int i=0; inbody; i++) { if (m->body_mocapid[i]>=0) { int mocapid = m->body_mocapid[i]; - mpos[3*mocapid] = m->body_pos[3*i]; - mpos[3*mocapid+1] = m->body_pos[3*i+1]; - mpos[3*mocapid+2] = m->body_pos[3*i+2]; + mpos_[3*mocapid] = m->body_pos[3*i]; + mpos_[3*mocapid+1] = m->body_pos[3*i+1]; + 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); } // mquat: allocate or check size - if (mquat.empty()) { - mquat.resize(4*m->nmocap); + if (mquat_.empty()) { + mquat_.resize(4*m->nmocap); if (m->nmocap) { for (int i=0; inbody; i++) { if (m->body_mocapid[i]>=0) { int mocapid = m->body_mocapid[i]; - mquat[4*mocapid] = m->body_quat[4*i]; - mquat[4*mocapid+1] = m->body_quat[4*i+1]; - mquat[4*mocapid+2] = m->body_quat[4*i+2]; - mquat[4*mocapid+3] = m->body_quat[4*i+3]; + mquat_[4*mocapid] = m->body_quat[4*i]; + mquat_[4*mocapid+1] = m->body_quat[4*i+1]; + mquat_[4*mocapid+2] = m->body_quat[4*i+2]; + 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); } // ctrl: allocate or check size - if (ctrl.empty()) { - ctrl.resize(m->nu); + if (ctrl_.empty()) { + ctrl_.resize(m->nu); for (int i=0; inu; 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); } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index affcef6e..322a88c4 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1235,18 +1235,25 @@ class mjCSensor : public mjCBase, private mjmSensor { //------------------------- class mjCNumeric ------------------------------------------------------- // Describes a custom data field -class mjCNumeric : public mjCBase { +class mjCNumeric : public mjCBase, private mjmNumeric { friend class mjCModel; + friend class mjXWriter; public: - // variables set by user - std::vector data; // initialization data - int size; // array size, can be bigger than data.size() + mjmNumeric spec; + using mjCBase::name; + using mjCBase::info; + + void PointToLocal(); + void CopyFromSpec(); private: mjCNumeric(mjCModel*); // constructor ~mjCNumeric(); // destructor void Compile(void); // compiler + + std::vector data_; + std::vector spec_data_; }; @@ -1254,17 +1261,25 @@ class mjCNumeric : public mjCBase { //------------------------- class mjCText ---------------------------------------------------------- // Describes a custom text field -class mjCText : public mjCBase { +class mjCText : public mjCBase, private mjmText { friend class mjCModel; + friend class mjXWriter; public: - // variables set by user - std::string data; // string + mjmText spec; + using mjCBase::name; + using mjCBase::info; + + void PointToLocal(); + void CopyFromSpec(); private: mjCText(mjCModel*); // constructor ~mjCText(); // destructor void Compile(void); // compiler + + std::string data_; + std::string spec_data_; }; @@ -1272,14 +1287,17 @@ class mjCText : public mjCBase { //------------------------- class mjCTuple --------------------------------------------------------- // Describes a custom tuple field -class mjCTuple : public mjCBase { +class mjCTuple : public mjCBase, private mjmTuple { friend class mjCModel; + friend class mjXWriter; public: - // variables set by user - std::vector objtype; // object types - std::vector objname; // object names - std::vector objprm; // object parameters + mjmTuple spec; + using mjCBase::name; + using mjCBase::info; + + void PointToLocal(); + void CopyFromSpec(); private: mjCTuple(mjCModel*); // constructor @@ -1287,6 +1305,14 @@ class mjCTuple : public mjCBase { void Compile(void); // compiler std::vector obj; // object pointers + + // variable-size data + std::vector objtype_; + std::vector objname_; + std::vector objprm_; + std::vector spec_objtype_; + std::vector spec_objname_; + std::vector spec_objprm_; }; @@ -1294,23 +1320,35 @@ class mjCTuple : public mjCBase { //------------------------- class mjCKey ----------------------------------------------------------- // Describes a keyframe -class mjCKey : public mjCBase { +class mjCKey : public mjCBase, private mjmKey { friend class mjCModel; friend class mjXWriter; public: - double time; // time - std::vector qpos; // qpos - std::vector qvel; // qvel - std::vector act; // act - std::vector mpos; // mocap pos - std::vector mquat; // mocap quat - std::vector ctrl; // ctrl + mjmKey spec; + using mjCBase::name; + using mjCBase::info; + + void PointToLocal(); + void CopyFromSpec(); private: mjCKey(mjCModel*); // constructor ~mjCKey(); // destructor void Compile(const mjModel* m); // compiler + + std::vector qpos_; + std::vector qvel_; + std::vector act_; + std::vector mpos_; + std::vector mquat_; + std::vector ctrl_; + std::vector spec_qpos_; + std::vector spec_qvel_; + std::vector spec_act_; + std::vector spec_mpos_; + std::vector spec_mquat_; + std::vector spec_ctrl_; }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 07ad8dbc..c40510c8 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -2781,15 +2781,20 @@ void mjXReader::Custom(XMLElement* section) { while (elem) { // get sub-element name name = elem->Value(); + string elname; // numeric if (name=="numeric") { // create custom - mjCNumeric* pnum = model->AddNumeric(); - GetXMLPos(elem, pnum); + mjmNumeric* pnum = mjm_addNumeric(model); + + // write error info + mjm_setString(pnum->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); // 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)) { int sz = pnum->size < 500 ? pnum->size : 500; for (int i=0; isize; i++) { - pnum->data.push_back(data[i]); - } + mjm_setDouble(pnum->data, data, pnum->size); } // text else if (name=="text") { // create custom - mjCText* pte = model->AddText(); - GetXMLPos(elem, pte); + mjmText* pte = mjm_addText(model); + + // write error info + mjm_setString(pte->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); // 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); if (text.empty()) { throw mjXError(elem, "text field cannot be empty"); } // copy data - pte->data = text; + mjm_setString(pte->data, text.c_str()); } // tuple else if (name=="tuple") { // create custom - mjCTuple* ptu = model->AddTuple(); - GetXMLPos(elem, ptu); + mjmTuple* ptu = mjm_addTuple(model); + + // write error info + mjm_setString(ptu->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); // read attributes - ReadAttrTxt(elem, "name", ptu->name, true); + ReadAttrTxt(elem, "name", elname, true); + mjm_setString(ptu->name, elname.c_str()); // read objects and add XMLElement* obj = FirstChildElement(elem); + std::vector objtype; + std::string objname = ""; + std::vector objprm; + while (obj) { // get sub-element name name = obj->Value(); @@ -2852,21 +2867,25 @@ void mjXReader::Custom(XMLElement* section) { if (otype==mjOBJ_UNKNOWN) { throw mjXError(obj, "unknown object type"); } - ptu->objtype.push_back(otype); + objtype.push_back(otype); // read name and assign ReadAttrTxt(obj, "objname", text, true); - ptu->objname.push_back(text); + objname += text + " "; // read parameter and assign double oprm = 0; ReadAttr(obj, "prm", 1, &oprm, text); - ptu->objprm.push_back(oprm); + objprm.push_back(oprm); } // advance to next object 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 @@ -3875,7 +3894,6 @@ void mjXReader::Sensor(XMLElement* section) { // keyframe section parser void mjXReader::Keyframe(XMLElement* section) { - string text; XMLElement* elem; int n; double data[1000]; @@ -3883,53 +3901,50 @@ void mjXReader::Keyframe(XMLElement* section) { // iterate over child elements elem = FirstChildElement(section); while (elem) { + string text, name = ""; + // add keyframe - mjCKey* pk = model->AddKey(); + mjmKey* pk = mjm_addKey(model); // 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); // read qpos n = ReadAttr(elem, "qpos", 1000, data, text, false, false); if (n) { - pk->qpos.resize(n); - mjuu_copyvec(pk->qpos.data(), data, n); + mjm_setDouble(pk->qpos, data, n); } // read qvel n = ReadAttr(elem, "qvel", 1000, data, text, false, false); if (n) { - pk->qvel.resize(n); - mjuu_copyvec(pk->qvel.data(), data, n); + mjm_setDouble(pk->qvel, data, n); } // read act n = ReadAttr(elem, "act", 1000, data, text, false, false); if (n) { - pk->act.resize(n); - mjuu_copyvec(pk->act.data(), data, n); + mjm_setDouble(pk->act, data, n); } // read mpos n = ReadAttr(elem, "mpos", 1000, data, text, false, false); if (n) { - pk->mpos.resize(n); - mjuu_copyvec(pk->mpos.data(), data, n); + mjm_setDouble(pk->mpos, data, n); } // read mquat n = ReadAttr(elem, "mquat", 1000, data, text, false, false); if (n) { - pk->mquat.resize(n); - mjuu_copyvec(pk->mquat.data(), data, n); + mjm_setDouble(pk->mquat, data, n); } // read ctrl n = ReadAttr(elem, "ctrl", 1000, data, text, false, false); if (n) { - pk->ctrl.resize(n); - mjuu_copyvec(pk->ctrl.data(), data, n); + mjm_setDouble(pk->ctrl, data, n); } // advance to next element diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 763ed1e4..c505ec05 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1283,7 +1283,7 @@ void mjXWriter::Custom(XMLElement* root) { elem = InsertEnd(section, "numeric"); WriteAttrTxt(elem, "name", ptr->name); WriteAttrInt(elem, "size", ptr->size); - WriteAttr(elem, "data", ptr->size, ptr->data.data()); + WriteAttr(elem, "data", ptr->size, ptr->data_.data()); } // write all texts @@ -1291,7 +1291,7 @@ void mjXWriter::Custom(XMLElement* root) { mjCText* ptr = (mjCText*)model->GetObject(mjOBJ_TEXT, i); elem = InsertEnd(section, "text"); WriteAttrTxt(elem, "name", ptr->name); - WriteAttrTxt(elem, "data", ptr->data.c_str()); + WriteAttrTxt(elem, "data", ptr->data_.c_str()); } // write all tuples @@ -1301,11 +1301,11 @@ void mjXWriter::Custom(XMLElement* root) { WriteAttrTxt(elem, "name", ptr->name); // 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"); - WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype[j])); - WriteAttrTxt(obj, "objname", ptr->objname[j].c_str()); - double oprm = ptr->objprm[j]; + WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype_[j])); + WriteAttrTxt(obj, "objname", ptr->objname_[j].c_str()); + double oprm = ptr->objprm_[j]; if (oprm!=0) { WriteAttr(obj, "prm", 1, &oprm); } @@ -1964,8 +1964,8 @@ void mjXWriter::Keyframe(XMLElement* root) { // check qpos and write for (int j=0; jnq; j++) { - if (pk->qpos[j]!=model->qpos0[j]) { - WriteAttr(elem, "qpos", model->nq, pk->qpos.data()); + if (pk->qpos_[j]!=model->qpos0[j]) { + WriteAttr(elem, "qpos", model->nq, pk->qpos_.data()); change = true; break; } @@ -1973,8 +1973,8 @@ void mjXWriter::Keyframe(XMLElement* root) { // check qvel and write for (int j=0; jnv; j++) { - if (pk->qvel[j]!=0) { - WriteAttr(elem, "qvel", model->nv, pk->qvel.data()); + if (pk->qvel_[j]!=0) { + WriteAttr(elem, "qvel", model->nv, pk->qvel_.data()); change = true; break; } @@ -1982,8 +1982,8 @@ void mjXWriter::Keyframe(XMLElement* root) { // check act and write for (int j=0; jna; j++) { - if (pk->act[j]!=0) { - WriteAttr(elem, "act", model->na, pk->act.data()); + if (pk->act_[j]!=0) { + WriteAttr(elem, "act", model->na, pk->act_.data()); change = true; break; } @@ -1995,10 +1995,10 @@ void mjXWriter::Keyframe(XMLElement* root) { if (model->bodies[j]->mocap) { mjCBody* pb = model->bodies[j]; int id = pb->mocapid; - if (pb->pos[0] != pk->mpos[3*id] || - pb->pos[1] != pk->mpos[3*id+1] || - pb->pos[2] != pk->mpos[3*id+2]) { - WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos.data()); + if (pb->pos[0] != pk->mpos_[3*id] || + pb->pos[1] != pk->mpos_[3*id+1] || + pb->pos[2] != pk->mpos_[3*id+2]) { + WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos_.data()); change = true; break; } @@ -2012,11 +2012,11 @@ void mjXWriter::Keyframe(XMLElement* root) { if (model->bodies[j]->mocap) { mjCBody* pb = model->bodies[j]; int id = pb->mocapid; - if (pb->quat[0] != pk->mquat[4*id] || - pb->quat[1] != pk->mquat[4*id+1] || - pb->quat[2] != pk->mquat[4*id+2] || - pb->quat[3] != pk->mquat[4*id+3]) { - WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat.data()); + if (pb->quat[0] != pk->mquat_[4*id] || + pb->quat[1] != pk->mquat_[4*id+1] || + pb->quat[2] != pk->mquat_[4*id+2] || + pb->quat[3] != pk->mquat_[4*id+3]) { + WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat_.data()); change = true; break; } @@ -2026,8 +2026,8 @@ void mjXWriter::Keyframe(XMLElement* root) { // check ctrl and write for (int j=0; jnu; j++) { - if (pk->ctrl[j]!=0) { - WriteAttr(elem, "ctrl", model->nu, pk->ctrl.data()); + if (pk->ctrl_[j]!=0) { + WriteAttr(elem, "ctrl", model->nu, pk->ctrl_.data()); change = true; break; }