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.
mjElement mjm_addPlugin(void* 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;
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
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
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()) {
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;
// 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
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
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
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; 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_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; i<nkey; i++) {
// copy data
m->key_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; i<nnumeric; i++) {
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
for (int i=0; i<ntuple; i++) {
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];
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);
}
}
src/user/user_objects.cc
+201 -63
View File
@@ -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<mjmNumeric*>(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<mjmText*>(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<mjmTuple*>(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; i<objtype.size(); i++) {
for (int i=0; i<objtype_.size(); i++) {
// find object by type and name
mjCBase* res = model->FindObject(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<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
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; 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);
}
// qvel: allocate or check size
if (qvel.empty()) {
qvel.resize(m->nv);
if (qvel_.empty()) {
qvel_.resize(m->nv);
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);
}
// act: allocate or check size
if (act.empty()) {
act.resize(m->na);
if (act_.empty()) {
act_.resize(m->na);
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);
}
// 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; i<m->nbody; 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; i<m->nbody; 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; 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);
}
}
src/user/user_objects.h
+58 -20
View File
@@ -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<double> 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<double> data_;
std::vector<double> 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<mjtObj> objtype; // object types
std::vector<std::string> objname; // object names
std::vector<double> 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<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 -----------------------------------------------------------
// 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<double> qpos; // qpos
std::vector<double> qvel; // qvel
std::vector<double> act; // act
std::vector<double> mpos; // mocap pos
std::vector<double> mquat; // mocap quat
std::vector<double> 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<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) {
// 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; i<sz; i++) {
@@ -2807,39 +2812,49 @@ void mjXReader::Custom(XMLElement* section) {
}
// copy data
for (int i=0; i<pnum->size; 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<int> objtype;
std::string objname = "";
std::vector<double> 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
src/xml/xml_native_writer.cc
+23 -23
View File
@@ -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; j<model->nq; 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; j<model->nv; 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; j<model->na; 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; j<model->nu; 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;
}