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

Add mjmActuator and mjmSensor.

Browse Source 
PiperOrigin-RevId: 605600704
Change-Id: I0a2f5eb9d894c068ca9ee08940c752066021b6ec
This commit is contained in:
Alessio Quaglino
2024-02-09 05:10:10 -08:00
committed by Copybara-Service
parent e3cbad280c
commit 6fa139a693
9 changed files with 453 additions and 273 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+19
View File
@@ -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<mjCModel*>(model);
mjCDef* def = static_cast<mjCDef*>(defspec);
mjCActuator* actuator = modelC->AddActuator(def);
return &actuator->spec;
}
// add sensor to model
mjmSensor* mjm_addSensor(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCSensor* sensor = modelC->AddSensor();
return &sensor->spec;
}
// Add plugin to model.
mjElement mjm_addPlugin(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model);
src/user/user_api.h
+65
View File
@@ -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
src/user/user_init.cc
+43
View File
@@ -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;
}
src/user/user_model.cc
+6 -6
View File
@@ -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; i<actuators.size(); i++) {
nuser_actuator = mjMAX(nuser_actuator, actuators[i]->userdata.size());
nuser_actuator = mjMAX(nuser_actuator, actuators[i]->spec_userdata_.size());
}
}
if (nuser_sensor == -1) {
nuser_sensor = 0;
for (int i=0; i<sensors.size(); i++) {
nuser_sensor = mjMAX(nuser_sensor, sensors[i]->userdata.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);
}
}
src/user/user_objects.cc
+109 -69
View File
@@ -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<mjmActuator*>(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<mjmSensor*>(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)",
src/user/user_objects.h
+45 -39
View File
@@ -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<double> 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<double>& 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<double> userdata_;
std::string spec_target_;
std::string spec_slidersite_;
std::string spec_refsite_;
std::vector<double> 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<double> 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<double>& 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<double> userdata_;
std::string spec_objname_;
std::string spec_refname_;
std::vector<double> spec_userdata_;
};
src/xml/xml_native_reader.cc
+112 -86
View File
@@ -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<double> 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<double> 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();
src/xml/xml_native_reader.h
+1 -1
View File
@@ -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);
src/xml/xml_native_writer.cc
+53 -72
View File
@@ -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());
}
}