diff --git a/src/user/user_api.cc b/src/user/user_api.cc index a73bd491..3dfc5db5 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -123,6 +123,25 @@ void* mjm_addFrame(mjmBody* bodyspec, void* parentframe) { +// add actuator to model +mjmActuator* mjm_addActuator(void* model, void* defspec) { + mjCModel* modelC = static_cast(model); + mjCDef* def = static_cast(defspec); + mjCActuator* actuator = modelC->AddActuator(def); + return &actuator->spec; +} + + + +// add sensor to model +mjmSensor* mjm_addSensor(void* model) { + mjCModel* modelC = static_cast(model); + mjCSensor* sensor = modelC->AddSensor(); + return &sensor->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 bd429ef3..d56d9191 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -211,6 +211,59 @@ typedef struct _mjmLight { } mjmLight; +typedef struct _mjmActuator { + mjElement element; // compiler only, do not modify + mjString name; // name + mjString classname; // class name + mjString info; // message appended to errors + int group; // group for visualization + int ctrllimited; // are control limits defined: 0 false, 1 true, 2 auto + int forcelimited; // are force limits defined: 0 false, 1 true, 2 auto + int actlimited; // are activation limits defined: 0 false, 1 true, 2 auto + int actdim; // dimension of associated activations + int plugin_actdim; // actuator state size for plugins + mjtDyn dyntype; // dynamics type + mjtTrn trntype; // transmission type + mjtGain gaintype; // gain type + mjtBias biastype; // bias type + double dynprm[mjNDYN]; // dynamics parameters + double gainprm[mjNGAIN]; // gain parameters + double biasprm[mjNGAIN]; // bias parameters + mjtByte actearly; // apply activations to qfrc instantly + double ctrlrange[2]; // control range + double forcerange[2]; // force range + double actrange[2]; // activation range + double lengthrange[2]; // length range + double gear[6]; // length and transmitted force scaling + double cranklength; // crank length, for slider-crank only + mjDouble userdata; // user data + mjString target; // transmission target name + mjString slidersite; // site defining cylinder, for slider-crank only + mjString refsite; // reference site, for site transmission only + mjmPlugin plugin; // actuator plugin +} mjmActuator; + + +typedef struct _mjmSensor { + mjElement element; // compiler only, do not modify + mjString name; // name + mjString classname; // class name + mjString info; // message appended to errors + mjtSensor type; // type of sensor + mjtDataType datatype; // data type for sensor measurement + mjtStage needstage; // compute stage needed to simulate sensor + mjtObj objtype; // type of sensorized object + mjString objname; // name of sensorized object + mjtObj reftype; // type of referenced object + mjString refname; // name of referenced object + int dim; // number of scalar outputs + double cutoff; // cutoff for real and positive datatypes + double noise; // noise stdev + mjDouble userdata; // user data + mjmPlugin plugin; // sensor plugin +} mjmSensor; + + //---------------------------------- Public API ---------------------------------------------------- // Create model. @@ -246,6 +299,12 @@ MJAPI mjmLight* mjm_addLight(mjmBody* body, void* defspec); // Add frame to body. MJAPI void* mjm_addFrame(mjmBody* body, void* parentframe); +// Add actuator to model. +MJAPI mjmActuator* mjm_addActuator(void* model, void* defspec); + +// Add sensor to model. +MJAPI mjmSensor* mjm_addSensor(void* model); + // Add plugin to model. MJAPI mjElement mjm_addPlugin(void* model); @@ -306,6 +365,12 @@ MJAPI void mjm_defaultCamera(mjmCamera& camera); // Default light attributes. MJAPI void mjm_defaultLight(mjmLight& light); +// Default actuator attributes. +MJAPI void mjm_defaultActuator(mjmActuator& actuator); + +// Default sensor attributes. +MJAPI void mjm_defaultSensor(mjmSensor& sensor); + #ifdef __cplusplus } #endif diff --git a/src/user/user_init.cc b/src/user/user_init.cc index 1272a0f7..e2af855f 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -81,6 +81,8 @@ void mjm_defaultGeom(mjmGeom& geom) { geom.fluid_coefs[2] = 1.5; // angular drag coefficient geom.fluid_coefs[3] = 1.0; // kutta lift coefficient geom.fluid_coefs[4] = 1.0; // magnus lift coefficient + geom.plugin.active = false; + geom.plugin.instance = nullptr; } @@ -131,3 +133,44 @@ void mjm_defaultLight(mjmLight& light) { } + +// default actuator attributes +void mjm_defaultActuator(mjmActuator& actuator) { + memset(&actuator, 0, sizeof(mjmActuator)); + actuator.group = 0; + actuator.ctrllimited = 2; + actuator.forcelimited = 2; + actuator.actlimited = 2; + actuator.actdim = -1; + actuator.plugin_actdim = 0; + actuator.trntype = mjTRN_UNDEFINED; + actuator.dyntype = mjDYN_NONE; + actuator.gaintype = mjGAIN_FIXED; + actuator.biastype = mjBIAS_NONE; + actuator.actearly = 0; + actuator.gear[0] = 1; + actuator.dynprm[0] = 1; + actuator.gainprm[0] = 1; + actuator.cranklength = 0; + actuator.plugin.active = false; + actuator.plugin.instance = nullptr; +} + + + +// default sensor attributes +void mjm_defaultSensor(mjmSensor& sensor) { + memset(&sensor, 0, sizeof(mjmSensor)); + sensor.type = mjSENS_TOUCH; + sensor.datatype = mjDATATYPE_REAL; + sensor.needstage = mjSTAGE_ACC; + sensor.objtype = mjOBJ_UNKNOWN; + sensor.reftype = mjOBJ_UNKNOWN; + sensor.cutoff = 0; + sensor.noise = 0; + sensor.dim = 0; +} + + + + diff --git a/src/user/user_model.cc b/src/user/user_model.cc index d82c2922..b7c1f0f9 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2298,7 +2298,7 @@ void mjCModel::CopyObjects(mjModel* m) { copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2); copyvec(m->actuator_actrange + 2*i, pac->actrange, 2); copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2); - copyvec(m->actuator_user+nuser_actuator*i, pac->userdata.data(), nuser_actuator); + copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator); } // sensors @@ -2318,7 +2318,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->sensor_dim[i] = psen->dim; m->sensor_cutoff[i] = (mjtNum)psen->cutoff; m->sensor_noise[i] = (mjtNum)psen->noise; - copyvec(m->sensor_user+nuser_sensor*i, psen->userdata.data(), nuser_sensor); + copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor); // calculate address and advance m->sensor_adr[i] = adr; @@ -2920,13 +2920,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { if (nuser_actuator == -1) { nuser_actuator = 0; for (int i=0; iuserdata.size()); + nuser_actuator = mjMAX(nuser_actuator, actuators[i]->spec_userdata_.size()); } } if (nuser_sensor == -1) { nuser_sensor = 0; for (int i=0; iuserdata.size()); + nuser_sensor = mjMAX(nuser_sensor, sensors[i]->spec_userdata_.size()); } } @@ -3500,7 +3500,7 @@ bool mjCModel::CopyBack(const mjModel* m) { pa->cranklength = (double)m->actuator_cranklength[i]; if (nuser_actuator) { - copyvec(pa->userdata.data(), m->actuator_user + nuser_actuator*i, nuser_actuator); + copyvec(pa->userdata_.data(), m->actuator_user + nuser_actuator*i, nuser_actuator); } } @@ -3510,7 +3510,7 @@ bool mjCModel::CopyBack(const mjModel* m) { sensors[i]->noise = (double)m->sensor_noise[i]; if (nuser_sensor) { - copyvec(sensors[i]->userdata.data(), m->sensor_user + nuser_sensor*i, nuser_sensor); + copyvec(sensors[i]->userdata_.data(), m->sensor_user + nuser_sensor*i, nuser_sensor); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 052a2eb9..3fb88a34 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -475,7 +475,7 @@ void mjCDef::Compile(const mjCModel* model) { site.userdata_.resize(model->nuser_site); camera.userdata_.resize(model->nuser_cam); tendon.userdata.resize(model->nuser_tendon); - actuator.userdata.resize(model->nuser_actuator); + actuator.userdata_.resize(model->nuser_actuator); } @@ -3963,35 +3963,13 @@ void mjCWrap::Compile(void) { // initialize defaults mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { - // actuator defaults - group = 0; - ctrllimited = 2; - forcelimited = 2; - actlimited = 2; - actdim = -1; - plugin_actdim = 0; - trntype = mjTRN_UNDEFINED; - dyntype = mjDYN_NONE; - gaintype = mjGAIN_FIXED; - biastype = mjBIAS_NONE; - mjuu_zerovec(dynprm, mjNDYN); - mjuu_zerovec(gainprm, mjNGAIN); - mjuu_zerovec(biasprm, mjNBIAS); - mjuu_zerovec(ctrlrange, 2); - mjuu_zerovec(forcerange, 2); - mjuu_zerovec(actrange, 2); - mjuu_zerovec(lengthrange, 2); - mjuu_zerovec(gear, 6); - gear[0] = 1; - dynprm[0] = 1; - gainprm[0] = 1; - cranklength = 0; - target.clear(); - slidersite.clear(); - refsite.clear(); - userdata.clear(); + mjm_defaultActuator(spec); // clear private variables + spec_target_.clear(); + spec_slidersite_.clear(); + spec_refsite_.clear(); + spec_userdata_.clear(); trnid[0] = trnid[1] = -1; // reset to default if given @@ -4003,23 +3981,59 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); + // in case this actuator is not compiled + CopyFromSpec(); + // point to local (needs to be after defaults) - plugin.name = (mjString)&plugin_name; - plugin.instance_name = (mjString)&plugin_instance_name; + MakePointerLocal(); +} + + + +void mjCActuator::MakePointerLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.userdata = (mjDouble)&spec_userdata_; + spec.target = (mjString)&spec_target_; + spec.refsite = (mjString)&spec_refsite_; + spec.slidersite = (mjString)&spec_slidersite_; + spec.plugin.name = (mjString)&plugin_name; + spec.plugin.instance_name = (mjString)&plugin_instance_name; + spec.info = (mjString)&info; +} + + + +void mjCActuator::CopyFromSpec() { + *static_cast(this) = spec; + userdata_ = spec_userdata_; + target_ = spec_target_; + refsite_ = spec_refsite_; + slidersite_ = spec_slidersite_; + userdata = (mjDouble)&userdata_; + target = (mjString)&target_; + refsite = (mjString)&refsite_; + slidersite = (mjString)&slidersite_; + plugin.active = spec.plugin.active; + plugin.instance = spec.plugin.instance; + plugin.name = spec.plugin.name; + plugin.instance_name = spec.plugin.instance_name; } // compiler void mjCActuator::Compile(void) { + CopyFromSpec(); mjCJoint* pjnt; // resize userdata - if (userdata.size() > model->nuser_actuator) { + if (userdata_.size() > model->nuser_actuator) { throw mjCError(this, "user has more values than nuser_actuator in actuator '%s' (id = %d)", name.c_str(), id); } - userdata.resize(model->nuser_actuator); + userdata_.resize(model->nuser_actuator); // if limited is auto, set to 1 if range is specified, otherwise unlimited if (forcelimited==2) { @@ -4104,7 +4118,7 @@ void mjCActuator::Compile(void) { } // check for missing target name - if (target.empty()) { + if (target_.empty()) { throw mjCError(this, "missing transmission target for actuator '%s' (id = %d)", name.c_str(), id); } @@ -4115,10 +4129,10 @@ void mjCActuator::Compile(void) { case mjTRN_JOINT: case mjTRN_JOINTINPARENT: // get joint - ptarget = model->FindObject(mjOBJ_JOINT, target); + ptarget = model->FindObject(mjOBJ_JOINT, target_); if (!ptarget) { throw mjCError(this, - "unknown transmission target '%s' for actuator id = %d", target.c_str(), id); + "unknown transmission target '%s' for actuator id = %d", target_.c_str(), id); } pjnt = (mjCJoint*) ptarget; @@ -4132,12 +4146,12 @@ void mjCActuator::Compile(void) { case mjTRN_SLIDERCRANK: // get slidersite, copy in trnid[1] - if (slidersite.empty()) { + if (slidersite_.empty()) { throw mjCError(this, "missing base site for slider-crank '%s' (id = %d)", name.c_str(), id); } - ptarget = model->FindObject(mjOBJ_SITE, slidersite); + ptarget = model->FindObject(mjOBJ_SITE, slidersite_); if (!ptarget) { - throw mjCError(this, "base site '%s' not found for actuator %d", slidersite.c_str(), id); + throw mjCError(this, "base site '%s' not found for actuator %d", slidersite_.c_str(), id); } trnid[1] = ptarget->id; @@ -4148,31 +4162,31 @@ void mjCActuator::Compile(void) { } // proceed with regular target - ptarget = model->FindObject(mjOBJ_SITE, target); + ptarget = model->FindObject(mjOBJ_SITE, target_); break; case mjTRN_TENDON: // get tendon - ptarget = model->FindObject(mjOBJ_TENDON, target); + ptarget = model->FindObject(mjOBJ_TENDON, target_); break; case mjTRN_SITE: // get refsite, copy into trnid[1] - if (!refsite.empty()) { - ptarget = model->FindObject(mjOBJ_SITE, refsite); + if (!refsite_.empty()) { + ptarget = model->FindObject(mjOBJ_SITE, refsite_); if (!ptarget) { - throw mjCError(this, "reference site '%s' not found for actuator %d", refsite.c_str(), id); + throw mjCError(this, "reference site '%s' not found for actuator %d", refsite_.c_str(), id); } trnid[1] = ptarget->id; } // proceed with regular site target - ptarget = model->FindObject(mjOBJ_SITE, target); + ptarget = model->FindObject(mjOBJ_SITE, target_); break; case mjTRN_BODY: // get body - ptarget = model->FindObject(mjOBJ_BODY, target); + ptarget = model->FindObject(mjOBJ_BODY, target_); break; default: @@ -4181,7 +4195,7 @@ void mjCActuator::Compile(void) { // assign and check if (!ptarget) { - throw mjCError(this, "transmission target '%s' not found in actuator %d", target.c_str(), id); + throw mjCError(this, "transmission target '%s' not found in actuator %d", target_.c_str(), id); } else { trnid[0] = ptarget->id; } @@ -4209,41 +4223,67 @@ void mjCActuator::Compile(void) { // initialize defaults mjCSensor::mjCSensor(mjCModel* _model) { + mjm_defaultSensor(spec); + // set model model = _model; - // set sensor defaults (somewhat arbitrary) - type = mjSENS_TOUCH; - datatype = mjDATATYPE_REAL; - needstage = mjSTAGE_ACC; - objtype = mjOBJ_UNKNOWN; - objname.clear(); - reftype = mjOBJ_UNKNOWN; - refname.clear(); - cutoff = 0; - noise = 0; - userdata.clear(); - dim = 0; - // clear private variables + spec_objname_.clear(); + spec_refname_.clear(); + spec_userdata_.clear(); obj = nullptr; refid = -1; - // point to local (needs to be after defaults) - plugin.name = (mjString)&plugin_name; - plugin.instance_name = (mjString)&plugin_instance_name; + // in case this sensor is not compiled + CopyFromSpec(); + + // point to local + MakePointerLocal(); +} + + + +void mjCSensor::MakePointerLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.userdata = (mjDouble)&spec_userdata_; + spec.objname = (mjString)&spec_objname_; + spec.refname = (mjString)&spec_refname_; + spec.plugin.name = (mjString)&plugin_name; + spec.plugin.instance_name = (mjString)&plugin_instance_name; + spec.info = (mjString)&info; +} + + + +void mjCSensor::CopyFromSpec() { + *static_cast(this) = spec; + userdata_ = spec_userdata_; + objname_ = spec_objname_; + refname_ = spec_refname_; + userdata = (mjDouble)&userdata_; + objname = (mjString)&objname_; + refname = (mjString)&refname_; + plugin.active = spec.plugin.active; + plugin.instance = spec.plugin.instance; + plugin.name = spec.plugin.name; + plugin.instance_name = spec.plugin.instance_name; } // compiler void mjCSensor::Compile(void) { + CopyFromSpec(); + // resize userdata - if (userdata.size() > model->nuser_sensor) { + if (userdata_.size() > model->nuser_sensor) { throw mjCError(this, "user has more values than nuser_sensor in sensor '%s' (id = %d)", name.c_str(), id); } - userdata.resize(model->nuser_sensor); + userdata_.resize(model->nuser_sensor); // require non-negative noise if (noise<0) { @@ -4258,14 +4298,14 @@ void mjCSensor::Compile(void) { // get objid from objtype and objname if (objtype!=mjOBJ_UNKNOWN) { // check for missing object name - if (objname.empty()) { + if (objname_.empty()) { throw mjCError(this, "missing name of sensorized object in sensor '%s' (id = %d)", name.c_str(), id); } // find name - obj = model->FindObject(objtype, objname); + obj = model->FindObject(objtype, objname_); if (!obj) { throw mjCError(this, "unrecognized name of sensorized object in sensor '%s' (id = %d)", @@ -4285,14 +4325,14 @@ void mjCSensor::Compile(void) { // get refid from reftype and refname if (reftype!=mjOBJ_UNKNOWN) { // check for missing object name - if (refname.empty()) { + if (refname_.empty()) { throw mjCError(this, "missing name of reference frame object in sensor '%s' (id = %d)", name.c_str(), id); } // find name - mjCBase* pref = model->FindObject(reftype, refname); + mjCBase* pref = model->FindObject(reftype, refname_); if (!pref) { throw mjCError(this, "unrecognized name of reference frame object in sensor '%s' (id = %d)", @@ -4351,7 +4391,7 @@ void mjCSensor::Compile(void) { // check for camera resolution for camera projection sensor if (type==mjSENS_CAMPROJECTION) { - mjCCamera* camref = (mjCCamera*) model->FindObject(mjOBJ_CAMERA, refname); + mjCCamera* camref = (mjCCamera*) model->FindObject(mjOBJ_CAMERA, refname_); if (!camref->resolution[0] || !camref->resolution[1]) { throw mjCError(this, "camera projection sensor requires camera resolution '%s' (id = %d)", diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 6ff9a30a..3cc25fbc 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1141,43 +1141,41 @@ class mjCPlugin : public mjCBase { //------------------------- class mjCActuator ------------------------------------------------------ // Describes an actuator -class mjCActuator : public mjCBase { +class mjCActuator : public mjCBase, private mjmActuator { friend class mjCDef; friend class mjCModel; friend class mjXWriter; public: - // variables set by user or API - int group; // group for visualization - int ctrllimited; // are control limits defined: 0 false, 1 true, 2 auto - int forcelimited; // are force limits defined: 0 false, 1 true, 2 auto - int actlimited; // are activation limits defined: 0 false, 1 true, 2 auto - int actdim; // dimension of associated activations - int plugin_actdim; // actuator state size for plugins - mjtDyn dyntype; // dynamics type - mjtTrn trntype; // transmission type - mjtGain gaintype; // gain type - mjtBias biastype; // bias type - double dynprm[mjNDYN]; // dynamics parameters - double gainprm[mjNGAIN]; // gain parameters - double biasprm[mjNGAIN]; // bias parameters - bool actearly = false; // apply activations to qfrc instantly - double ctrlrange[2]; // control range - double forcerange[2]; // force range - double actrange[2]; // activation range - double lengthrange[2]; // length range - double gear[6]; // length and transmitted force scaling - double cranklength; // crank length, for slider-crank only - std::vector userdata; // user data - std::string target; // transmission target name - std::string slidersite; // site defining cylinder, for slider-crank only - std::string refsite; // reference site, for site transmission only + mjmActuator spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; + using mjCBase::plugin; + + // used by mjXWriter and mjCModel + const std::vector& get_userdata() { return userdata_; } + const std::string& get_target() { return spec_target_; } + const std::string& get_slidersite() { return spec_slidersite_; } + const std::string& get_refsite() { return spec_refsite_; } private: mjCActuator(mjCModel* = 0, mjCDef* = 0); // constructor void Compile(void); // compiler + void CopyFromSpec(); + void MakePointerLocal(); int trnid[2]; // id of transmission target + + // variable-size data + std::string target_; + std::string slidersite_; + std::string refsite_; + std::vector userdata_; + std::string spec_target_; + std::string spec_slidersite_; + std::string spec_refsite_; + std::vector spec_userdata_; }; @@ -1185,31 +1183,39 @@ class mjCActuator : public mjCBase { //------------------------- class mjCSensor -------------------------------------------------------- // Describes a sensor -class mjCSensor : public mjCBase { +class mjCSensor : public mjCBase, private mjmSensor { friend class mjCDef; friend class mjCModel; friend class mjXWriter; public: - // variables set by user or API - mjtSensor type; // type of sensor - mjtDataType datatype; // data type for sensor measurement - mjtStage needstage; // compute stage needed to simulate sensor - mjtObj objtype; // type of sensorized object - std::string objname; // name of sensorized object - mjtObj reftype; - std::string refname; - int dim; // number of scalar outputs - double cutoff; // cutoff for real and positive datatypes - double noise; // noise stdev - std::vector userdata; // user data + mjmSensor spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; + using mjCBase::plugin; + + // used by mjXWriter and mjCModel + const std::vector& get_userdata() { return userdata_; } + const std::string& get_objname() { return spec_objname_; } + const std::string& get_refname() { return spec_refname_; } private: mjCSensor(mjCModel*); // constructor void Compile(void); // compiler + void CopyFromSpec(); + void MakePointerLocal(); mjCBase* obj; // sensorized object int refid; // id of reference frame + + // variable-size data + std::string objname_; + std::string refname_; + std::vector userdata_; + std::string spec_objname_; + std::string spec_refname_; + std::vector spec_userdata_; }; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 5fb376ca..4387868a 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1853,12 +1853,16 @@ void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) { // actuator element parser -void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { - string text, type; +void mjXReader::OneActuator(XMLElement* elem, mjmActuator* pact) { + string text, type, name, classname, target, slidersite, refsite; // common attributes - ReadAttrTxt(elem, "name", pact->name); - ReadAttrTxt(elem, "class", pact->classname); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pact->name, name.c_str()); + } + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(pact->classname, classname.c_str()); + } ReadAttrInt(elem, "group", &pact->group); MapValue(elem, "ctrllimited", &pact->ctrllimited, TFAuto_map, 3); MapValue(elem, "forcelimited", &pact->forcelimited, TFAuto_map, 3); @@ -1871,27 +1875,33 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { // transmission target and type int cnt = 0; - if (ReadAttrTxt(elem, "joint", pact->target)) { + if (ReadAttrTxt(elem, "joint", target)) { + mjm_setString(pact->target, target.c_str()); pact->trntype = mjTRN_JOINT; cnt++; } - if (ReadAttrTxt(elem, "jointinparent", pact->target)) { + if (ReadAttrTxt(elem, "jointinparent", target)) { + mjm_setString(pact->target, target.c_str()); pact->trntype = mjTRN_JOINTINPARENT; cnt++; } - if (ReadAttrTxt(elem, "tendon", pact->target)) { + if (ReadAttrTxt(elem, "tendon", target)) { + mjm_setString(pact->target, target.c_str()); pact->trntype = mjTRN_TENDON; cnt++; } - if (ReadAttrTxt(elem, "cranksite", pact->target)) { + if (ReadAttrTxt(elem, "cranksite", target)) { + mjm_setString(pact->target, target.c_str()); pact->trntype = mjTRN_SLIDERCRANK; cnt++; } - if (ReadAttrTxt(elem, "site", pact->target)) { + if (ReadAttrTxt(elem, "site", target)) { + mjm_setString(pact->target, target.c_str()); pact->trntype = mjTRN_SITE; cnt++; } - if (ReadAttrTxt(elem, "body", pact->target)) { + if (ReadAttrTxt(elem, "body", target)) { + mjm_setString(pact->target, target.c_str()); pact->trntype = mjTRN_BODY; cnt++; } @@ -1902,13 +1912,19 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { // slidercrank-specific parameters int r1 = ReadAttr(elem, "cranklength", 1, &pact->cranklength, text); - int r2 = ReadAttrTxt(elem, "slidersite", pact->slidersite); + int r2 = ReadAttrTxt(elem, "slidersite", slidersite); + if (r2) { + mjm_setString(pact->slidersite, slidersite.c_str()); + } if ((r1 || r2) && pact->trntype!=mjTRN_SLIDERCRANK && pact->trntype!=mjTRN_UNDEFINED) { throw mjXError(elem, "cranklength and slidersite can only be used in slidercrank transmission"); } // site-specific parameters (refsite) - int r3 = ReadAttrTxt(elem, "refsite", pact->refsite); + int r3 = ReadAttrTxt(elem, "refsite", refsite); + if (r3) { + mjm_setString(pact->refsite, refsite.c_str()); + } if (r3 && pact->trntype!=mjTRN_SITE && pact->trntype!=mjTRN_UNDEFINED) { throw mjXError(elem, "refsite can only be used with site transmission"); } @@ -2103,9 +2119,14 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { } // read userdata - ReadVector(elem, "user", pact->userdata, text); + std::vector userdata; + if (ReadVector(elem, "user", userdata, text)) { + mjm_setDouble(pact->userdata, userdata.data(), userdata.size()); + } - GetXMLPos(elem, pact); + // write info + mjm_setString(pact->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -2571,7 +2592,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { name=="cylinder" || name=="muscle" || name=="adhesion") { - OneActuator(elem, &def->actuator); + OneActuator(elem, &def->actuator.spec); } // copy into private attributes @@ -2580,6 +2601,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { mjm_finalize(def->site.spec.element); mjm_finalize(def->camera.spec.element); mjm_finalize(def->light.spec.element); + mjm_finalize(def->actuator.spec.element); // advance elem = elem->NextSiblingElement(); @@ -3423,7 +3445,7 @@ void mjXReader::Actuator(XMLElement* section) { } // create actuator and parse - mjCActuator* pact = model->AddActuator(def); + mjmActuator* pact = mjm_addActuator(model, def); OneActuator(elem, pact); // advance to next element @@ -3436,133 +3458,136 @@ void mjXReader::Actuator(XMLElement* section) { // sensor section parser void mjXReader::Sensor(XMLElement* section) { int n; - string text; XMLElement* elem = section->FirstChildElement(); while (elem) { // create sensor, get string type - mjCSensor* psen = model->AddSensor(); + mjmSensor* psen = mjm_addSensor(model); string type = elem->Value(); - string plugin_name = ""; - string instance_name = ""; + string text, name, objname, refname; + std::vector userdata; // read name, noise, userdata - ReadAttrTxt(elem, "name", psen->name); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(psen->name, name.c_str()); + } ReadAttr(elem, "cutoff", 1, &psen->cutoff, text); ReadAttr(elem, "noise", 1, &psen->noise, text); - ReadVector(elem, "user", psen->userdata, text); + if (ReadVector(elem, "user", userdata, text)) { + mjm_setDouble(psen->userdata, userdata.data(), userdata.size()); + } // common robotic sensors, attached to a site if (type=="touch") { psen->type = mjSENS_TOUCH; psen->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", psen->objname, true); + ReadAttrTxt(elem, "site", objname, true); } else if (type=="accelerometer") { psen->type = mjSENS_ACCELEROMETER; psen->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", psen->objname, true); + ReadAttrTxt(elem, "site", objname, true); } else if (type=="velocimeter") { psen->type = mjSENS_VELOCIMETER; psen->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", psen->objname, true); + ReadAttrTxt(elem, "site", objname, true); } else if (type=="gyro") { psen->type = mjSENS_GYRO; psen->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", psen->objname, true); + ReadAttrTxt(elem, "site", objname, true); } else if (type=="force") { psen->type = mjSENS_FORCE; psen->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", psen->objname, true); + ReadAttrTxt(elem, "site", objname, true); } else if (type=="torque") { psen->type = mjSENS_TORQUE; psen->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", psen->objname, true); + ReadAttrTxt(elem, "site", objname, true); } else if (type=="magnetometer") { psen->type = mjSENS_MAGNETOMETER; psen->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", psen->objname, true); + ReadAttrTxt(elem, "site", objname, true); } else if (type=="camprojection") { psen->type = mjSENS_CAMPROJECTION; psen->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", psen->objname, true); - ReadAttrTxt(elem, "camera", psen->refname, true); + ReadAttrTxt(elem, "site", objname, true); + ReadAttrTxt(elem, "camera", refname, true); psen->reftype = mjOBJ_CAMERA; } else if (type=="rangefinder") { psen->type = mjSENS_RANGEFINDER; psen->objtype = mjOBJ_SITE; - ReadAttrTxt(elem, "site", psen->objname, true); + ReadAttrTxt(elem, "site", objname, true); } // sensors related to scalar joints, tendons, actuators else if (type=="jointpos") { psen->type = mjSENS_JOINTPOS; psen->objtype = mjOBJ_JOINT; - ReadAttrTxt(elem, "joint", psen->objname, true); + ReadAttrTxt(elem, "joint", objname, true); } else if (type=="jointvel") { psen->type = mjSENS_JOINTVEL; psen->objtype = mjOBJ_JOINT; - ReadAttrTxt(elem, "joint", psen->objname, true); + ReadAttrTxt(elem, "joint", objname, true); } else if (type=="tendonpos") { psen->type = mjSENS_TENDONPOS; psen->objtype = mjOBJ_TENDON; - ReadAttrTxt(elem, "tendon", psen->objname, true); + ReadAttrTxt(elem, "tendon", objname, true); } else if (type=="tendonvel") { psen->type = mjSENS_TENDONVEL; psen->objtype = mjOBJ_TENDON; - ReadAttrTxt(elem, "tendon", psen->objname, true); + ReadAttrTxt(elem, "tendon", objname, true); } else if (type=="actuatorpos") { psen->type = mjSENS_ACTUATORPOS; psen->objtype = mjOBJ_ACTUATOR; - ReadAttrTxt(elem, "actuator", psen->objname, true); + ReadAttrTxt(elem, "actuator", objname, true); } else if (type=="actuatorvel") { psen->type = mjSENS_ACTUATORVEL; psen->objtype = mjOBJ_ACTUATOR; - ReadAttrTxt(elem, "actuator", psen->objname, true); + ReadAttrTxt(elem, "actuator", objname, true); } else if (type=="actuatorfrc") { psen->type = mjSENS_ACTUATORFRC; psen->objtype = mjOBJ_ACTUATOR; - ReadAttrTxt(elem, "actuator", psen->objname, true); + ReadAttrTxt(elem, "actuator", objname, true); } else if (type=="jointactuatorfrc") { psen->type = mjSENS_JOINTACTFRC; psen->objtype = mjOBJ_JOINT; - ReadAttrTxt(elem, "joint", psen->objname, true); + ReadAttrTxt(elem, "joint", objname, true); } // sensors related to ball joints else if (type=="ballquat") { psen->type = mjSENS_BALLQUAT; psen->objtype = mjOBJ_JOINT; - ReadAttrTxt(elem, "joint", psen->objname, true); + ReadAttrTxt(elem, "joint", objname, true); } else if (type=="ballangvel") { psen->type = mjSENS_BALLANGVEL; psen->objtype = mjOBJ_JOINT; - ReadAttrTxt(elem, "joint", psen->objname, true); + ReadAttrTxt(elem, "joint", objname, true); } // joint and tendon limit sensors else if (type=="jointlimitpos") { psen->type = mjSENS_JOINTLIMITPOS; psen->objtype = mjOBJ_JOINT; - ReadAttrTxt(elem, "joint", psen->objname, true); + ReadAttrTxt(elem, "joint", objname, true); } else if (type=="jointlimitvel") { psen->type = mjSENS_JOINTLIMITVEL; psen->objtype = mjOBJ_JOINT; - ReadAttrTxt(elem, "joint", psen->objname, true); + ReadAttrTxt(elem, "joint", objname, true); } else if (type=="jointlimitfrc") { psen->type = mjSENS_JOINTLIMITFRC; psen->objtype = mjOBJ_JOINT; - ReadAttrTxt(elem, "joint", psen->objname, true); + ReadAttrTxt(elem, "joint", objname, true); } else if (type=="tendonlimitpos") { psen->type = mjSENS_TENDONLIMITPOS; psen->objtype = mjOBJ_TENDON; - ReadAttrTxt(elem, "tendon", psen->objname, true); + ReadAttrTxt(elem, "tendon", objname, true); } else if (type=="tendonlimitvel") { psen->type = mjSENS_TENDONLIMITVEL; psen->objtype = mjOBJ_TENDON; - ReadAttrTxt(elem, "tendon", psen->objname, true); + ReadAttrTxt(elem, "tendon", objname, true); } else if (type=="tendonlimitfrc") { psen->type = mjSENS_TENDONLIMITFRC; psen->objtype = mjOBJ_TENDON; - ReadAttrTxt(elem, "tendon", psen->objname, true); + ReadAttrTxt(elem, "tendon", objname, true); } // sensors attached to an object with spatial frame: (x)body, geom, site, camera @@ -3570,10 +3595,10 @@ void mjXReader::Sensor(XMLElement* section) { psen->type = mjSENS_FRAMEPOS; ReadAttrTxt(elem, "objtype", text, true); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname, true); + ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { psen->reftype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "refname", psen->refname, true); + ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } @@ -3581,10 +3606,10 @@ void mjXReader::Sensor(XMLElement* section) { psen->type = mjSENS_FRAMEQUAT; ReadAttrTxt(elem, "objtype", text, true); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname, true); + ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { psen->reftype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "refname", psen->refname, true); + ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } @@ -3592,10 +3617,10 @@ void mjXReader::Sensor(XMLElement* section) { psen->type = mjSENS_FRAMEXAXIS; ReadAttrTxt(elem, "objtype", text, true); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname, true); + ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { psen->reftype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "refname", psen->refname, true); + ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } @@ -3603,10 +3628,10 @@ void mjXReader::Sensor(XMLElement* section) { psen->type = mjSENS_FRAMEYAXIS; ReadAttrTxt(elem, "objtype", text, true); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname, true); + ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { psen->reftype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "refname", psen->refname, true); + ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } @@ -3614,10 +3639,10 @@ void mjXReader::Sensor(XMLElement* section) { psen->type = mjSENS_FRAMEZAXIS; ReadAttrTxt(elem, "objtype", text, true); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname, true); + ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { psen->reftype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "refname", psen->refname, true); + ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } @@ -3625,10 +3650,10 @@ void mjXReader::Sensor(XMLElement* section) { psen->type = mjSENS_FRAMELINVEL; ReadAttrTxt(elem, "objtype", text, true); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname, true); + ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { psen->reftype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "refname", psen->refname, true); + ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } @@ -3636,10 +3661,10 @@ void mjXReader::Sensor(XMLElement* section) { psen->type = mjSENS_FRAMEANGVEL; ReadAttrTxt(elem, "objtype", text, true); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname, true); + ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { psen->reftype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "refname", psen->refname, true); + ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } @@ -3647,27 +3672,27 @@ void mjXReader::Sensor(XMLElement* section) { psen->type = mjSENS_FRAMELINACC; ReadAttrTxt(elem, "objtype", text, true); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname, true); + ReadAttrTxt(elem, "objname", objname, true); } else if (type=="frameangacc") { psen->type = mjSENS_FRAMEANGACC; ReadAttrTxt(elem, "objtype", text, true); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname, true); + ReadAttrTxt(elem, "objname", objname, true); } // sensors related to kinematic subtrees; attached to a body (which is the subtree root) else if (type=="subtreecom") { psen->type = mjSENS_SUBTREECOM; psen->objtype = mjOBJ_BODY; - ReadAttrTxt(elem, "body", psen->objname, true); + ReadAttrTxt(elem, "body", objname, true); } else if (type=="subtreelinvel") { psen->type = mjSENS_SUBTREELINVEL; psen->objtype = mjOBJ_BODY; - ReadAttrTxt(elem, "body", psen->objname, true); + ReadAttrTxt(elem, "body", objname, true); } else if (type=="subtreeangmom") { psen->type = mjSENS_SUBTREEANGMOM; psen->objtype = mjOBJ_BODY; - ReadAttrTxt(elem, "body", psen->objname, true); + ReadAttrTxt(elem, "body", objname, true); } // global sensors @@ -3679,14 +3704,14 @@ void mjXReader::Sensor(XMLElement* section) { // user-defined sensor else if (type=="user") { psen->type = mjSENS_USER; - bool objname_given = ReadAttrTxt(elem, "objname", psen->objname); + bool objname_given = ReadAttrTxt(elem, "objname", objname); if (ReadAttrTxt(elem, "objtype", text)) { if (!objname_given) { throw mjXError(elem, "objtype '%s' given but objname is missing", text.c_str()); } psen->objtype = (mjtObj)mju_str2Type(text.c_str()); } else if (objname_given) { - throw mjXError(elem, "objname '%s' given but objtype is missing", psen->objname.c_str()); + throw mjXError(elem, "objname '%s' given but objtype is missing", objname.c_str()); } ReadAttrInt(elem, "dim", &psen->dim, true); @@ -3701,38 +3726,39 @@ void mjXReader::Sensor(XMLElement* section) { else if (type=="plugin") { psen->type = mjSENS_PLUGIN; - ReadAttrTxt(elem, "plugin", plugin_name); - ReadAttrTxt(elem, "instance", instance_name); - mjm_setString(psen->plugin.name, plugin_name.c_str()); - mjm_setString(psen->plugin.instance_name, instance_name.c_str()); - if (instance_name.empty()) { - psen->plugin.instance = mjm_addPlugin(model); - } else { - model->hasImplicitPluginElem = true; - } - ReadPluginConfigs(elem, (mjCPlugin*)psen->plugin.instance); + OnePlugin(elem, &psen->plugin); ReadAttrTxt(elem, "objtype", text); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); - ReadAttrTxt(elem, "objname", psen->objname); - if (psen->objtype != mjOBJ_UNKNOWN && psen->objname.empty()) { + ReadAttrTxt(elem, "objname", objname); + if (psen->objtype != mjOBJ_UNKNOWN && objname.empty()) { throw mjXError(elem, "objtype is specified but objname is not"); } - if (psen->objtype == mjOBJ_UNKNOWN && !psen->objname.empty()) { + if (psen->objtype == mjOBJ_UNKNOWN && !objname.empty()) { throw mjXError(elem, "objname is specified but objtype is not"); } if (ReadAttrTxt(elem, "reftype", text)) { psen->reftype = (mjtObj)mju_str2Type(text.c_str()); } - ReadAttrTxt(elem, "refname", psen->refname); - if (psen->reftype != mjOBJ_UNKNOWN && psen->refname.empty()) { + ReadAttrTxt(elem, "refname", refname); + if (psen->reftype != mjOBJ_UNKNOWN && refname.empty()) { throw mjXError(elem, "reftype is specified but refname is not"); } - if (psen->reftype == mjOBJ_UNKNOWN && !psen->refname.empty()) { + if (psen->reftype == mjOBJ_UNKNOWN && !refname.empty()) { throw mjXError(elem, "refname is specified but reftype is not"); } } - GetXMLPos(elem, psen); + if (!objname.empty()) { + mjm_setString(psen->objname, objname.c_str()); + } + + if (!refname.empty()) { + mjm_setString(psen->refname, refname.c_str()); + } + + // write info + mjm_setString(psen->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); // advance to next element elem = elem->NextSiblingElement(); diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 38387d12..ce69a21b 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -65,7 +65,7 @@ class mjXReader : public mjXBase { void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair); void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality); void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon); - void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator); + void OneActuator(tinyxml2::XMLElement* elem, mjmActuator* pactuator); void OneComposite(tinyxml2::XMLElement* elem, mjmBody* pbody, mjCDef* def); void OneFlexcomp(tinyxml2::XMLElement* elem, mjmBody* pbody); void OnePlugin(tinyxml2::XMLElement* elem, mjmPlugin* plugin); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 4d9955a0..408dcd1a 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -664,29 +664,29 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { // transmission target switch (pact->trntype) { case mjTRN_JOINT: - WriteAttrTxt(elem, "joint", pact->target); + WriteAttrTxt(elem, "joint", pact->get_target()); break; case mjTRN_JOINTINPARENT: - WriteAttrTxt(elem, "jointinparent", pact->target); + WriteAttrTxt(elem, "jointinparent", pact->get_target()); break; case mjTRN_TENDON: - WriteAttrTxt(elem, "tendon", pact->target); + WriteAttrTxt(elem, "tendon", pact->get_target()); break; case mjTRN_SLIDERCRANK: - WriteAttrTxt(elem, "cranksite", pact->target); - WriteAttrTxt(elem, "slidersite", pact->slidersite); + WriteAttrTxt(elem, "cranksite", pact->get_target()); + WriteAttrTxt(elem, "slidersite", pact->get_slidersite()); break; case mjTRN_SITE: - WriteAttrTxt(elem, "site", pact->target); - WriteAttrTxt(elem, "refsite", pact->refsite); + WriteAttrTxt(elem, "site", pact->get_target()); + WriteAttrTxt(elem, "refsite", pact->get_refsite()); break; case mjTRN_BODY: - WriteAttrTxt(elem, "body", pact->target); + WriteAttrTxt(elem, "body", pact->get_target()); break; default: // SHOULD NOT OCCUR @@ -747,9 +747,9 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { // userdata if (writingdefaults) { - WriteVector(elem, "user", pact->userdata); + WriteVector(elem, "user", pact->get_userdata()); } else { - WriteVector(elem, "user", pact->userdata, def->actuator.userdata); + WriteVector(elem, "user", pact->get_userdata(), def->actuator.get_userdata()); } } @@ -1748,171 +1748,171 @@ void mjXWriter::Sensor(XMLElement* root) { // common robotic sensors, attached to a site case mjSENS_TOUCH: elem = InsertEnd(section, "touch"); - WriteAttrTxt(elem, "site", psen->objname); + WriteAttrTxt(elem, "site", psen->get_objname()); break; case mjSENS_ACCELEROMETER: elem = InsertEnd(section, "accelerometer"); - WriteAttrTxt(elem, "site", psen->objname); + WriteAttrTxt(elem, "site", psen->get_objname()); break; case mjSENS_VELOCIMETER: elem = InsertEnd(section, "velocimeter"); - WriteAttrTxt(elem, "site", psen->objname); + WriteAttrTxt(elem, "site", psen->get_objname()); break; case mjSENS_GYRO: elem = InsertEnd(section, "gyro"); - WriteAttrTxt(elem, "site", psen->objname); + WriteAttrTxt(elem, "site", psen->get_objname()); break; case mjSENS_FORCE: elem = InsertEnd(section, "force"); - WriteAttrTxt(elem, "site", psen->objname); + WriteAttrTxt(elem, "site", psen->get_objname()); break; case mjSENS_TORQUE: elem = InsertEnd(section, "torque"); - WriteAttrTxt(elem, "site", psen->objname); + WriteAttrTxt(elem, "site", psen->get_objname()); break; case mjSENS_MAGNETOMETER: elem = InsertEnd(section, "magnetometer"); - WriteAttrTxt(elem, "site", psen->objname); + WriteAttrTxt(elem, "site", psen->get_objname()); break; case mjSENS_RANGEFINDER: elem = InsertEnd(section, "rangefinder"); - WriteAttrTxt(elem, "site", psen->objname); + WriteAttrTxt(elem, "site", psen->get_objname()); break; case mjSENS_CAMPROJECTION: elem = InsertEnd(section, "camprojection"); - WriteAttrTxt(elem, "site", psen->objname); - WriteAttrTxt(elem, "camera", psen->refname); + WriteAttrTxt(elem, "site", psen->get_objname()); + WriteAttrTxt(elem, "camera", psen->get_refname()); break; // sensors related to scalar joints, tendons, actuators case mjSENS_JOINTPOS: elem = InsertEnd(section, "jointpos"); - WriteAttrTxt(elem, "joint", psen->objname); + WriteAttrTxt(elem, "joint", psen->get_objname()); break; case mjSENS_JOINTVEL: elem = InsertEnd(section, "jointvel"); - WriteAttrTxt(elem, "joint", psen->objname); + WriteAttrTxt(elem, "joint", psen->get_objname()); break; case mjSENS_TENDONPOS: elem = InsertEnd(section, "tendonpos"); - WriteAttrTxt(elem, "tendon", psen->objname); + WriteAttrTxt(elem, "tendon", psen->get_objname()); break; case mjSENS_TENDONVEL: elem = InsertEnd(section, "tendonvel"); - WriteAttrTxt(elem, "tendon", psen->objname); + WriteAttrTxt(elem, "tendon", psen->get_objname()); break; case mjSENS_ACTUATORPOS: elem = InsertEnd(section, "actuatorpos"); - WriteAttrTxt(elem, "actuator", psen->objname); + WriteAttrTxt(elem, "actuator", psen->get_objname()); break; case mjSENS_ACTUATORVEL: elem = InsertEnd(section, "actuatorvel"); - WriteAttrTxt(elem, "actuator", psen->objname); + WriteAttrTxt(elem, "actuator", psen->get_objname()); break; case mjSENS_ACTUATORFRC: elem = InsertEnd(section, "actuatorfrc"); - WriteAttrTxt(elem, "actuator", psen->objname); + WriteAttrTxt(elem, "actuator", psen->get_objname()); break; case mjSENS_JOINTACTFRC: elem = InsertEnd(section, "jointactuatorfrc"); - WriteAttrTxt(elem, "joint", psen->objname); + WriteAttrTxt(elem, "joint", psen->get_objname()); break; // sensors related to ball joints case mjSENS_BALLQUAT: elem = InsertEnd(section, "ballquat"); - WriteAttrTxt(elem, "joint", psen->objname); + WriteAttrTxt(elem, "joint", psen->get_objname()); break; case mjSENS_BALLANGVEL: elem = InsertEnd(section, "ballangvel"); - WriteAttrTxt(elem, "joint", psen->objname); + WriteAttrTxt(elem, "joint", psen->get_objname()); break; // joint and tendon limit sensors case mjSENS_JOINTLIMITPOS: elem = InsertEnd(section, "jointlimitpos"); - WriteAttrTxt(elem, "joint", psen->objname); + WriteAttrTxt(elem, "joint", psen->get_objname()); break; case mjSENS_JOINTLIMITVEL: elem = InsertEnd(section, "jointlimitvel"); - WriteAttrTxt(elem, "joint", psen->objname); + WriteAttrTxt(elem, "joint", psen->get_objname()); break; case mjSENS_JOINTLIMITFRC: elem = InsertEnd(section, "jointlimitfrc"); - WriteAttrTxt(elem, "joint", psen->objname); + WriteAttrTxt(elem, "joint", psen->get_objname()); break; case mjSENS_TENDONLIMITPOS: elem = InsertEnd(section, "tendonlimitpos"); - WriteAttrTxt(elem, "tendon", psen->objname); + WriteAttrTxt(elem, "tendon", psen->get_objname()); break; case mjSENS_TENDONLIMITVEL: elem = InsertEnd(section, "tendonlimitvel"); - WriteAttrTxt(elem, "tendon", psen->objname); + WriteAttrTxt(elem, "tendon", psen->get_objname()); break; case mjSENS_TENDONLIMITFRC: elem = InsertEnd(section, "tendonlimitfrc"); - WriteAttrTxt(elem, "tendon", psen->objname); + WriteAttrTxt(elem, "tendon", psen->get_objname()); break; // sensors attached to an object with spatial frame: (x)body, geom, site, camera case mjSENS_FRAMEPOS: elem = InsertEnd(section, "framepos"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); break; case mjSENS_FRAMEQUAT: elem = InsertEnd(section, "framequat"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); break; case mjSENS_FRAMEXAXIS: elem = InsertEnd(section, "framexaxis"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); break; case mjSENS_FRAMEYAXIS: elem = InsertEnd(section, "frameyaxis"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); break; case mjSENS_FRAMEZAXIS: elem = InsertEnd(section, "framezaxis"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); break; case mjSENS_FRAMELINVEL: elem = InsertEnd(section, "framelinvel"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); break; case mjSENS_FRAMEANGVEL: elem = InsertEnd(section, "frameangvel"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); break; case mjSENS_FRAMELINACC: elem = InsertEnd(section, "framelinacc"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); break; case mjSENS_FRAMEANGACC: elem = InsertEnd(section, "frameangacc"); WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); break; // sensors related to kinematic subtrees; attached to a body (which is the subtree root) case mjSENS_SUBTREECOM: elem = InsertEnd(section, "subtreecom"); - WriteAttrTxt(elem, "body", psen->objname); + WriteAttrTxt(elem, "body", psen->get_objname()); break; case mjSENS_SUBTREELINVEL: elem = InsertEnd(section, "subtreelinvel"); - WriteAttrTxt(elem, "body", psen->objname); + WriteAttrTxt(elem, "body", psen->get_objname()); break; case mjSENS_SUBTREEANGMOM: elem = InsertEnd(section, "subtreeangmom"); - WriteAttrTxt(elem, "body", psen->objname); + WriteAttrTxt(elem, "body", psen->get_objname()); break; // global sensors @@ -1926,35 +1926,16 @@ void mjXWriter::Sensor(XMLElement* root) { elem = InsertEnd(section, "plugin"); if (psen->objtype != mjOBJ_UNKNOWN) { WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); - } - instance_name = std::string(mjm_getString(psen->plugin.instance_name)); - plugin_name = std::string(mjm_getString(psen->plugin.name)); - if (!instance_name.empty()) { - WriteAttrTxt(elem, "instance", instance_name); - } else { - WriteAttrTxt(elem, "plugin", plugin_name); - const mjpPlugin* plugin = mjp_getPluginAtSlot( - ((mjCPlugin*)psen->plugin.instance)->plugin_slot); - const char* c = &((mjCPlugin*)psen->plugin.instance)->flattened_attributes[0]; - for (int i = 0; i < plugin->nattribute; ++i) { - std::string value(c); - if (!value.empty()) { - XMLElement* config_elem = InsertEnd(elem, "config"); - WriteAttrTxt(config_elem, "key", plugin->attributes[i]); - WriteAttrTxt(config_elem, "value", value); - c += value.size(); - } - ++c; - } + WriteAttrTxt(elem, "objname", psen->get_objname()); } + OnePlugin(elem, psen); break; // user-defined sensor case mjSENS_USER: elem = InsertEnd(section, "user"); if (mju_type2Str(psen->objtype)) WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->objname); + WriteAttrTxt(elem, "objname", psen->get_objname()); WriteAttrInt(elem, "dim", psen->dim); WriteAttrKey(elem, "needstage", stage_map, stage_sz, (int)psen->needstage); WriteAttrKey(elem, "datatype", datatype_map, datatype_sz, (int)psen->datatype); @@ -1970,12 +1951,12 @@ void mjXWriter::Sensor(XMLElement* root) { if (psen->type != mjSENS_PLUGIN) { WriteAttr(elem, "noise", 1, &psen->noise, &zero); } - WriteVector(elem, "user", psen->userdata); + WriteVector(elem, "user", psen->get_userdata()); // add reference if present if (psen->reftype != mjOBJ_UNKNOWN && psen->type != mjSENS_CAMPROJECTION) { WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->refname); + WriteAttrTxt(elem, "refname", psen->get_refname()); } }