diff --git a/src/user/user_api.cc b/src/user/user_api.cc index c32b779b..edfda080 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -1,4 +1,4 @@ -// Copyright 2021 DeepMind Technologies Limited +// Copyright 2024 DeepMind Technologies Limited // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -31,48 +31,159 @@ void* mjm_createModel() { // delete model -void mjm_deleteModel(void* model) { +void mjm_deleteModel(void* modelspec) { + mjCModel* model = static_cast(modelspec); + delete model; +} + + + +// add child body to body, return child spec +mjmBody* mjm_addBody(mjmBody* bodyspec, void* defspec) { + mjCDef* def = static_cast(defspec); + mjCBody* body = reinterpret_cast(bodyspec->element)->AddBody(def); + return &body->spec; +} + + + +// add site to body, return site spec +mjmSite* mjm_addSite(mjmBody* bodyspec, void* defspec) { + mjCDef* def = static_cast(defspec); + mjCBody* body = reinterpret_cast(bodyspec->element); + mjCSite* site = body->AddSite(def); + return &site->spec; +} + + + +// add joint to body +void* mjm_addJoint(mjmBody* bodyspec, void* defspec) { + mjCDef* def = static_cast(defspec); + mjCBody* body = reinterpret_cast(bodyspec->element); + mjCJoint* joint = body->AddJoint(def); + return joint; +} + + + +// add free joint to body +void* mjm_addFreeJoint(mjmBody* bodyspec) { + mjCBody* body = reinterpret_cast(bodyspec->element); + mjCJoint* joint = body->AddFreeJoint(); + return joint; +} + + + +// add geom to body +void* mjm_addGeom(mjmBody* bodyspec, void* defspec) { + mjCDef* def = static_cast(defspec); + mjCBody* body = reinterpret_cast(bodyspec->element); + mjCGeom* geom = body->AddGeom(def); + return geom; +} + + + +// add camera to body +void* mjm_addCamera(mjmBody* bodyspec, void* defspec) { + mjCDef* def = static_cast(defspec); + mjCBody* body = reinterpret_cast(bodyspec->element); + mjCCamera* camera = body->AddCamera(def); + return camera; +} + + + +// add light to body +void* mjm_addLight(mjmBody* bodyspec, void* defspec) { + mjCDef* def = static_cast(defspec); + mjCBody* body = reinterpret_cast(bodyspec->element); + mjCLight* light = body->AddLight(def); + return light; +} + + + +// add frame to body +void* mjm_addFrame(mjmBody* bodyspec, void* parentframe) { + mjCFrame* parentframeC = static_cast(parentframe); + mjCFrame* frameC = reinterpret_cast(bodyspec->element)->AddFrame(parentframeC); + return frameC; +} + + + +// Add plugin to model. +mjElement mjm_addPlugin(void* model) { mjCModel* modelC = static_cast(model); - delete modelC; + mjCPlugin* plugin = modelC->AddPlugin(); + return (mjElement)plugin; } -// add body to body -void* mjm_addBody(void* body, void* def) { - mjCDef* defC = static_cast(def); - mjCBody* bodyC = static_cast(body); - return bodyC->AddBody(defC); +// get objects +void* mjm_getModel(mjmBody* bodyspec) { + return reinterpret_cast(bodyspec->element)->model; } -// add site to body -mjmSite* mjm_addSite(void* body, void* def) { - mjCDef* defC = static_cast(def); - mjCSite* siteC = static_cast(body)->AddSite(defC); - return &siteC->spec; +// get default +void* mjm_getDefault(mjElement element) { + return reinterpret_cast(element)->def; } -// get object of given type -void* mjm_findObject(void* model, mjtObj type, const char* name) { - mjCModel* modelC = static_cast(model); - return modelC->FindObject(type, std::string(name)); +// find body in model by name +mjmBody* mjm_findBody(void* modelspec, const char* name) { + mjCModel* model = static_cast(modelspec); + mjCBase* body = model->FindObject(mjOBJ_BODY, std::string(name)); + if (!body) { + return 0; + } + return &(static_cast(body)->spec); } -// set parent frame of dest -void mjm_setFrame(void* dest, void* frame) { +// find child of a body by name +mjmBody* mjm_findChild(mjmBody* bodyspec, const char* name) { + mjCBody* body = reinterpret_cast(bodyspec->element); + mjCBase* child = body->FindObject(mjOBJ_BODY, std::string(name)); + if (!child) { + return 0; + } + return &(static_cast(child)->spec); +} + + + +// set frame +void mjm_setFrame(mjElement dest, void* frame) { mjCFrame* frameC = static_cast(frame); - mjCBase* baseC = static_cast(dest); + mjCBase* baseC = reinterpret_cast(dest); baseC->SetFrame(frameC); } +// get id +int mjm_getId(mjElement element) { + return reinterpret_cast(element)->id; +} + + +// set default +void mjm_setDefault(mjElement element, void* defspec) { + mjCBase* baseC = reinterpret_cast(element); + baseC->def = static_cast(defspec); +} + + // set string void mjm_setString(mjString dest, const char* text) { std::string* str = reinterpret_cast(dest); @@ -108,3 +219,13 @@ const double* mjm_getDouble(const mjDouble source, int* size) { } return v->data(); } + + + +// compute full inertia +const char* mjm_setFullInertia(mjmBody* bodyspec, double quat[4], double inertia[3]) { + mjCBody* body = reinterpret_cast(bodyspec->element); + return body->FullInertia(quat, inertia); +} + + diff --git a/src/user/user_api.h b/src/user/user_api.h index 8a983b32..8b30b485 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -1,4 +1,4 @@ -// Copyright 2021 DeepMind Technologies Limited +// Copyright 2024 DeepMind Technologies Limited // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,14 +17,20 @@ #include #include +#include typedef struct _mjString* mjString; typedef struct _mjDouble* mjDouble; typedef struct _mjElement* mjElement; +// this is a C-API +#ifdef __cplusplus +extern "C" { +#endif //---------------------------------- Public structs ------------------------------------------------ + typedef struct _mjmOrientation { double axisangle[4]; // rotation axis and angle double xyaxes[6]; // x and y axes @@ -32,42 +38,104 @@ typedef struct _mjmOrientation { double euler[3]; // euler rotations } mjmOrientation; -typedef struct _mjmSite { - mjElement element; // only used internally, do not modify + +typedef struct _mjmPlugin { + bool active; + mjString name; + mjString instance_name; + mjElement instance; +} mjmPlugin; + + +typedef struct _mjmBody { + mjElement element; // compiler only, do not modify mjString name; // name mjString classname; // class name + double pos[3]; // frame position + double quat[4]; // frame orientation + mjtByte mocap; // is this a mocap body + mjmOrientation alt; // frame alternative orientation + double gravcomp; // gravity compensation + mjDouble userdata; // user data + double ipos[3]; // inertial frame position + double iquat[4]; // inertial frame orientation + double mass; // mass + double inertia[3]; // diagonal inertia (in i-frame) + mjmOrientation ialt; // inertial frame alternative orientation + double fullinertia[6]; // non-axis-aligned inertia matrix + mjtByte explicitinertial; // whether to save the body with an explicit inertial clause + mjmPlugin plugin; // passive force plugin mjString info; // message appended to errors +} mjmBody; + + +typedef struct _mjmSite { + mjElement element; // compiler only, do not modify + mjString name; // name + mjString classname; // class name mjtGeom type; // geom type for rendering int group; // group id, used for visualization - double size[3]; // geom size for rendering double pos[3]; // position double quat[4]; // orientation mjString material; // name of material for rendering - mjDouble userdata; // user data - float rgba[4]; // rgba when material is omitted + double size[3]; // geom size for rendering double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid mjmOrientation alt; // alternative orientation specification + float rgba[4]; // rgba when material is omitted + mjDouble userdata; // user data + mjString info; // message appended to errors } mjmSite; + //---------------------------------- Public API ---------------------------------------------------- -#ifdef __cplusplus -extern "C" { -#endif // Create model. MJAPI void* mjm_createModel(); // Delete model. -MJAPI void mjm_deleteModel(void* model); +MJAPI void mjm_deleteModel(void* modelspec); -// Add body to body. -MJAPI void* mjm_addBody(void* body, void* def); +// Add child body to body, return child spec. +MJAPI mjmBody* mjm_addBody(mjmBody* body, void* defspec); -// Add site to body. -MJAPI mjmSite* mjm_addSite(void* body, void* def); +// Add site to body, return site spec. +MJAPI mjmSite* mjm_addSite(mjmBody* body, void* defspec); -// Find object of given type. -MJAPI void* mjm_findObject(void* model, mjtObj type, const char* name); +// Add joint to body. +MJAPI void* mjm_addJoint(mjmBody* body, void* defspec); + +// Add freejoint to body. +MJAPI void* mjm_addFreeJoint(mjmBody* body); + +// Add geom to body. +MJAPI void* mjm_addGeom(mjmBody* body, void* defspec); + +// Add camera to body. +MJAPI void* mjm_addCamera(mjmBody* body, void* defspec); + +// Add light to body. +MJAPI void* mjm_addLight(mjmBody* body, void* defspec); + +// Add frame to body. +MJAPI void* mjm_addFrame(mjmBody* body, void* parentframe); + +// Add plugin to model. +MJAPI mjElement mjm_addPlugin(void* model); + +// Get model from body. +MJAPI void* mjm_getModel(mjmBody* body); + +// Get default corresponding to an mjElement. +MJAPI void* mjm_getDefault(mjElement element); + +// Finding body with given name in model. +MJAPI mjmBody* mjm_findBody(void* modelspec, const char* name); + +// Finding body with given name in body. +MJAPI mjmBody* mjm_findChild(mjmBody* body, const char* name); + +// Get element id. +MJAPI int mjm_getId(mjElement element); // Copy input text to destination string. MJAPI void mjm_setString(mjString dest, const char* text); @@ -75,14 +143,20 @@ MJAPI void mjm_setString(mjString dest, const char* text); // Copy input array to destination vector. MJAPI void mjm_setDouble(mjDouble dest, const double* array, int size); -// Get const pointer to mjString data. +// Get string contents. MJAPI const char* mjm_getString(mjString source); -// Get const pointer to mjDouble data and its size. +// Get double array contents and optionally its size. MJAPI const double* mjm_getDouble(mjDouble source, int* size); +// Set default. +MJAPI void mjm_setDefault(mjElement element, void* defspec); + // Set frame. -MJAPI void mjm_setFrame(void* dest, void* frame); +MJAPI void mjm_setFrame(mjElement dest, void* frame); + +// Compute quat and inertia from body->fullinertia.. +MJAPI const char* mjm_setFullInertia(mjmBody* body, double quat[4], double inertia[3]); #ifdef __cplusplus } diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 08496987..02a0d0c9 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "user/user_api.h" #include "user/user_composite.h" #include @@ -216,7 +217,7 @@ void mjCComposite::SetDefault(void) { // make composite object -bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_sz) { +bool mjCComposite::Make(mjCModel* model, mjmBody* body, char* error, int error_sz) { // check geom type if ((def[0].geom.type!=mjGEOM_SPHERE && def[0].geom.type!=mjGEOM_CAPSULE && @@ -326,7 +327,7 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s -bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int error_sz) { +bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int error_sz) { char txt[100]; std::vector face; @@ -439,13 +440,13 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int // create bodies and geoms for (int i=0; iAddBody(NULL); + mjmBody* b = mjm_addBody(body, NULL); if (!username.empty()) { - b->name = username[i]; + mjm_setString(b->name, username[i].c_str()); } else { mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i); - b->name = txt; + mjm_setString(b->name, txt); } // set body position @@ -456,8 +457,8 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int // add slider joints if none defined if (!add[mjCOMPKIND_PARTICLE]) { for (int i=0; i<3; i++) { - mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); - jnt->def = body->def; + mjCJoint* jnt = (mjCJoint*)mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0]); + jnt->def = (mjCDef*)mjm_getDefault(body->element); jnt->type = mjJNT_SLIDE; mjuu_setvec(jnt->pos, 0, 0, 0); mjuu_setvec(jnt->axis, 0, 0, 0); @@ -468,35 +469,36 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int // add user-specified joints else { for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) { - mjCJoint* jnt = b->AddJoint(&defjnt); - jnt->def = body->def; + mjCJoint* jnt = (mjCJoint*)mjm_addJoint(b, &defjnt); + jnt->def = (mjCDef*)mjm_getDefault(body->element); } } // add geom - mjCGeom* g = b->AddGeom(def); - g->def = body->def; + mjCGeom* g = (mjCGeom*)mjm_addGeom(b, def); + g->def = (mjCDef*)mjm_getDefault(body->element); // add site - mjCSite* s = b->AddSite(def); - s->def = body->def; - s->spec.type = mjGEOM_SPHERE; + mjmSite* s = mjm_addSite(b, def); + mjm_setDefault(s->element, mjm_getDefault(body->element)); + s->type = mjGEOM_SPHERE; mju::sprintf_arr(txt, "%sS%d", prefix.c_str(), i); - s->name = txt; + mjm_setString(s->name, txt); // add plugin if (plugin_instance) { - b->is_plugin = true; - b->plugin_name = plugin_name; - b->plugin_instance = plugin_instance; - b->plugin_instance_name = plugin_instance_name; + mjmPlugin* plugin = &b->plugin; + plugin->active = true; + plugin->instance = (mjElement)plugin_instance; + mjm_setString(plugin->instance_name, plugin_instance_name.c_str()); + mjm_setString(plugin->name, plugin_name.c_str()); if (i==0 && !plugin_instance->config_attribs["face"].empty()) { return comperr(error, "Face attribute already exists in plugin", error_sz); } - b->plugin_instance->config_attribs["face"] = userface; - b->plugin_instance->config_attribs["edge"] = ""; + plugin_instance->config_attribs["face"] = userface; + plugin_instance->config_attribs["edge"] = ""; // update density if (dim == 2) { @@ -566,7 +568,7 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int // make grid connected with tendons -bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int error_sz) { +bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int error_sz) { char txt[100], txt1[100], txt2[100]; // check dimensionality @@ -588,9 +590,9 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err for (int ix=0; ixAddBody(NULL); + mjmBody* b = mjm_addBody(body, NULL); mju::sprintf_arr(txt, "%sB%d_%d", prefix.c_str(), ix, iy); - b->name = txt; + mjm_setString(b->name, txt); // set body position b->pos[0] = offset[0] + spacing*(ix - 0.5*count[0]); @@ -598,18 +600,18 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err b->pos[2] = offset[2]; // add geom - mjCGeom* g = b->AddGeom(def); - g->def = body->def; + mjCGeom* g = (mjCGeom*)mjm_addGeom(b, def); + g->def = (mjCDef*)mjm_getDefault(body->element); g->type = mjGEOM_SPHERE; mju::sprintf_arr(txt, "%sG%d_%d", prefix.c_str(), ix, iy); g->name = txt; // add site - mjCSite* s = b->AddSite(def); - s->def = body->def; - s->spec.type = mjGEOM_SPHERE; + mjmSite* s = mjm_addSite(b, def); + mjm_setDefault(s->element, mjm_getDefault(body->element)); + s->type = mjGEOM_SPHERE; mju::sprintf_arr(txt, "%sS%d_%d", prefix.c_str(), ix, iy); - s->name = txt; + mjm_setString(s->name, txt); // skip pinned elements bool skip = false; @@ -626,8 +628,8 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err // add slider joint mjCJoint* jnt[3]; for (int i=0; i<3; i++) { - jnt[i] = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); - jnt[i]->def = body->def; + jnt[i] = (mjCJoint*)mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0]); + jnt[i]->def = (mjCDef*)mjm_getDefault(body->element); mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), i, ix, iy); jnt[i]->name = txt; jnt[i]->type = mjJNT_SLIDE; @@ -682,7 +684,7 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err -bool mjCComposite::MakeCable(mjCModel* model, mjCBody* body, char* error, int error_sz) { +bool mjCComposite::MakeCable(mjCModel* model, mjmBody* body, char* error, int error_sz) { // check dim if (dim!=1) { return comperr(error, "Cable must be one-dimensional", error_sz); @@ -753,7 +755,7 @@ bool mjCComposite::MakeCable(mjCModel* model, mjCBody* body, char* error, int er -mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]) { +mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]) { char txt_geom[100], txt_site[100], txt_slide[100]; char this_body[100], next_body[100], this_joint[100]; mjtNum dquat[4], this_quat[4]; @@ -809,8 +811,8 @@ mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtN mju::sprintf_arr(txt_slide, "%sJs%d", prefix.c_str(), ix); // add body - body = body->AddBody(); - body->name = this_body; + body = mjm_addBody(body, 0); + mjm_setString(body->name, this_body); if (first) { mjuu_setvec(body->pos, offset[0]+uservert[3*ix], offset[1]+uservert[3*ix+1], @@ -824,8 +826,8 @@ mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtN } // add geom - mjCGeom* geom = body->AddGeom(def); - geom->def = body->def; + mjCGeom* geom = (mjCGeom*)mjm_addGeom(body, def); + geom->def = (mjCDef*)mjm_getDefault(body->element); geom->name = txt_geom; if (def[0].geom.type==mjGEOM_CYLINDER || def[0].geom.type==mjGEOM_CAPSULE) { @@ -839,10 +841,11 @@ mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtN // add plugin if (plugin_instance) { - body->is_plugin = true; - body->plugin_name = plugin_name; - body->plugin_instance = plugin_instance; - body->plugin_instance_name = plugin_instance_name; + mjmPlugin* plugin = &body->plugin; + plugin->active = true; + plugin->instance = (mjElement)plugin_instance; + mjm_setString(plugin->name, plugin_name.c_str()); + mjm_setString(plugin->instance_name, plugin_instance_name.c_str()); } // update orientation @@ -850,8 +853,8 @@ mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtN // add curvature joint if (!first || strcmp(initial.c_str(), "none")) { - mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); - jnt->def = body->def; + mjCJoint* jnt = (mjCJoint*)mjm_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0]); + jnt->def = (mjCDef*)mjm_getDefault(body->element); jnt->type = (first && strcmp(initial.c_str(), "free")==0) ? mjJNT_FREE : mjJNT_BALL; jnt->damping = jnt->type==mjJNT_FREE ? 0 : jnt->damping; jnt->armature = jnt->type==mjJNT_FREE ? 0 : jnt->armature; @@ -868,11 +871,11 @@ mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtN // add site at the boundary if (last || first) { - mjCSite* site = body->AddSite(def); - site->def = body->def; - site->name = txt_site; - mjuu_setvec(site->spec.pos, last ? length : 0, 0, 0); - mjuu_setvec(site->spec.quat, 1, 0, 0, 0); + mjmSite* site = mjm_addSite(body, def); + mjm_setDefault(site->element, mjm_getDefault(body->element)); + mjm_setString(site->name, txt_site); + mjuu_setvec(site->pos, last ? length : 0, 0, 0); + mjuu_setvec(site->quat, 1, 0, 0, 0); } return body; @@ -880,7 +883,7 @@ mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtN // make rope -bool mjCComposite::MakeRope(mjCModel* model, mjCBody* body, char* error, int error_sz) { +bool mjCComposite::MakeRope(mjCModel* model, mjmBody* body, char* error, int error_sz) { // check dim if (dim!=1) { return comperr(error, "Rope must be one-dimensional", error_sz); @@ -889,13 +892,14 @@ bool mjCComposite::MakeRope(mjCModel* model, mjCBody* body, char* error, int err // check root body name prefix char txt[200]; mju::sprintf_arr(txt, "%sB", prefix.c_str()); - if (std::strncmp(txt, body->name.substr(0, strlen(txt)).c_str(), mju::sizeof_arr(txt))) { + std::string body_name = mjm_getString(body->name); + if (std::strncmp(txt, body_name.substr(0, strlen(txt)).c_str(), mju::sizeof_arr(txt))) { mju::strcat_arr(txt, " must be the beginning of root body name"); return comperr(error, txt, error_sz); } // read origin coordinate from root body - mju::strcpy_arr(txt, body->name.substr(strlen(txt)).c_str()); + mju::strcpy_arr(txt, body_name.substr(strlen(txt)).c_str()); int ox = -1; if (sscanf(txt, "%d", &ox)!=1) { return comperr(error, "Root body name must contain X coordinate", error_sz); @@ -908,7 +912,7 @@ bool mjCComposite::MakeRope(mjCModel* model, mjCBody* body, char* error, int err AddRopeBody(model, body, ox, ox); // add elements: right - mjCBody* pbody = body; + mjmBody* pbody = body; for (int ix=ox; ixAddBody(); + body = mjm_addBody(body, 0); mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), ix1); - body->name = txt; + mjm_setString(body->name, txt); // loop if (type==mjCOMPTYPE_LOOP) { @@ -978,8 +982,8 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i } // add geom - mjCGeom* geom = body->AddGeom(def); - geom->def = body->def; + mjCGeom* geom = (mjCGeom*)mjm_addGeom(body, def); + geom->def = (mjCDef*)mjm_getDefault(body->element); mju::sprintf_arr(txt, "%sG%d", prefix.c_str(), ix1); geom->name = txt; mjuu_setvec(geom->pos, 0, 0, 0); @@ -993,8 +997,8 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i // add main joint for (int i=0; i<2; i++) { // add joint - mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); - jnt->def = body->def; + mjCJoint* jnt = (mjCJoint*)mjm_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0]); + jnt->def = (mjCDef*)mjm_getDefault(body->element); mju::sprintf_arr(txt, "%sJ%d_%d", prefix.c_str(), i, ix1); jnt->name = txt; jnt->type = mjJNT_HINGE; @@ -1006,8 +1010,8 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i // add twist joint if (add[mjCOMPKIND_TWIST]) { // add joint - mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_TWIST][0]); - jnt->def = body->def; + mjCJoint* jnt = (mjCJoint*)mjm_addJoint(body, &defjoint[mjCOMPKIND_TWIST][0]); + jnt->def = (mjCDef*)mjm_getDefault(body->element); mju::sprintf_arr(txt, "%sJT%d", prefix.c_str(), ix1); jnt->name = txt; jnt->type = mjJNT_HINGE; @@ -1024,8 +1028,8 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i // add stretch joint if (add[mjCOMPKIND_STRETCH]) { // add joint - mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_STRETCH][0]); - jnt->def = body->def; + mjCJoint* jnt = (mjCJoint*)mjm_addJoint(body, &defjoint[mjCOMPKIND_STRETCH][0]); + jnt->def = (mjCDef*)mjm_getDefault(body->element); mju::sprintf_arr(txt, "%sJS%d", prefix.c_str(), ix1); jnt->name = txt; jnt->type = mjJNT_SLIDE; @@ -1081,7 +1085,7 @@ void mjCComposite::BoxProject(double* pos) { // make 3d box, ellipsoid or cylinder -bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int error_sz) { +bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int error_sz) { char txt[100]; // check dim @@ -1090,8 +1094,8 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro } // center geom: two times bigger - mjCGeom* geom = body->AddGeom(def); - geom->def = body->def; + mjCGeom* geom = (mjCGeom*)mjm_addGeom(body, def); + geom->def = (mjCDef*)mjm_getDefault(body->element); geom->type = mjGEOM_SPHERE; mju::sprintf_arr(txt, "%sGcenter", prefix.c_str()); geom->name = txt; @@ -1114,9 +1118,9 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro iy==0 || iy==count[1]-1 || iz==0 || iz==count[2]-1) { // create body - mjCBody* b = body->AddBody(NULL); + mjmBody* b = mjm_addBody(body, NULL); mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz); - b->name = txt; + mjm_setString(b->name, txt); // set body position (+/- 1) b->pos[0] = 2.0*ix/(count[0]-1) - 1; @@ -1131,8 +1135,8 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro mjuu_normvec(b->alt.zaxis, 3); // add geom - mjCGeom* g = b->AddGeom(def); - g->def = body->def; + mjCGeom* g = (mjCGeom*) mjm_addGeom(b, def); + g->def = (mjCDef*)mjm_getDefault(body->element); mju::sprintf_arr(txt, "%sG%d_%d_%d", prefix.c_str(), ix, iy, iz); g->name = txt; @@ -1145,8 +1149,8 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro } // add slider joint - mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); - jnt->def = body->def; + mjCJoint* jnt = (mjCJoint*)mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0]); + jnt->def = (mjCDef*)mjm_getDefault(body->element); mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), ix, iy, iz); jnt->name = txt; jnt->type = mjJNT_SLIDE; diff --git a/src/user/user_composite.h b/src/user/user_composite.h index 8510aace..d85e5891 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -68,13 +68,13 @@ class mjCComposite { bool AddDefaultJoint(char* error = NULL, int error_sz = 0); void AdjustSoft(mjtNum* solref, mjtNum* solimp, int level); - bool Make(mjCModel* model, mjCBody* body, char* error, int error_sz); + bool Make(mjCModel* model, mjmBody* body, char* error, int error_sz); - bool MakeParticle(mjCModel* model, mjCBody* body, char* error, int error_sz); - bool MakeGrid(mjCModel* model, mjCBody* body, char* error, int error_sz); - bool MakeRope(mjCModel* model, mjCBody* body, char* error, int error_sz); - bool MakeCable(mjCModel* model, mjCBody* body, char* error, int error_sz); - bool MakeBox(mjCModel* model, mjCBody* body, char* error, int error_sz); + bool MakeParticle(mjCModel* model, mjmBody* body, char* error, int error_sz); + bool MakeGrid(mjCModel* model, mjmBody* body, char* error, int error_sz); + bool MakeRope(mjCModel* model, mjmBody* body, char* error, int error_sz); + bool MakeCable(mjCModel* model, mjmBody* body, char* error, int error_sz); + bool MakeBox(mjCModel* model, mjmBody* body, char* error, int error_sz); void MakeShear(mjCModel* model); void MakeSkin2(mjCModel* model, mjtNum inflate); @@ -136,8 +136,8 @@ class mjCComposite { int dim; // dimensionality private: - mjCBody* AddRopeBody(mjCModel* model, mjCBody* body, int ix, int ix1); - mjCBody* AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]); + mjmBody* AddRopeBody(mjCModel* model, mjmBody* body, int ix, int ix1); + mjmBody* AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]); }; #endif // MUJOCO_SRC_USER_USER_COMPOSITE_H_ diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index da8d4e43..15842320 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "user/user_api.h" #include "user/user_flexcomp.h" #include @@ -76,7 +77,7 @@ mjCFlexcomp::mjCFlexcomp(void) { // make flexcomp object -bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz) { +bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz) { bool radial = (type==mjFCOMPTYPE_BOX || type==mjFCOMPTYPE_CYLINDER || type==mjFCOMPTYPE_ELLIPSOID); @@ -85,7 +86,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz type==mjFCOMPTYPE_GMSH); // check parent body name - if (body->name.empty()) { + if (std::string(mjm_getString(body->name)).empty()) { return comperr(error, "Parent body must have name", error_sz); } @@ -381,7 +382,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz // rigid: set parent name, nothing else to do if (rigid) { - pf->vertbody.push_back(body->name); + pf->vertbody.push_back(std::string(mjm_getString(body->name)).c_str()); return true; } @@ -398,21 +399,22 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz // pinned: parent body if (pinned[i]) { - pf->vertbody.push_back(body->name); + pf->vertbody.push_back(std::string(mjm_getString(body->name)).c_str()); // add plugin if (plugin_instance) { - body->is_plugin = true; - body->plugin_name = plugin_name; - body->plugin_instance = plugin_instance; - body->plugin_instance_name = plugin_instance_name; + mjmPlugin* plugin = &body->plugin; + plugin->active = true; + plugin->instance = (mjElement)plugin_instance; + mjm_setString(plugin->name, plugin_name.c_str()); + mjm_setString(plugin->instance_name, plugin_instance_name.c_str()); } } // not pinned: new body else { // add new body at vertex coordinates - mjCBody* pb = body->AddBody(); + mjmBody* pb = mjm_addBody(body, 0); // set frame and inertial pb->pos[0] = point[3*i]; @@ -423,11 +425,11 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz pb->inertia[0] = bodyinertia; pb->inertia[1] = bodyinertia; pb->inertia[2] = bodyinertia; - pb->MakeInertialExplicit(); + pb->explicitinertial = true; // add radial slider if (radial) { - mjCJoint* jnt = pb->AddJoint(); + mjCJoint* jnt = (mjCJoint*)mjm_addJoint(pb, 0); // set properties jnt->type = mjJNT_SLIDE; @@ -440,7 +442,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz else { for (int j=0; j<3; j++) { // add joint to body - mjCJoint* jnt = pb->AddJoint(); + mjCJoint* jnt = (mjCJoint*)mjm_addJoint(pb, 0); // set properties jnt->type = mjJNT_SLIDE; @@ -453,8 +455,8 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz // construct body name, add to vertbody char txt[100]; mju::sprintf_arr(txt, "%s_%d", name.c_str(), i); - pb->name = txt; - pf->vertbody.push_back(pb->name); + mjm_setString(pb->name, txt); + pf->vertbody.push_back(std::string(mjm_getString(pb->name)).c_str()); // clear flex vertex coordinates if allocated if (!centered) { @@ -465,10 +467,11 @@ bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz // add plugin if (plugin_instance) { - pb->is_plugin = true; - pb->plugin_name = plugin_name; - pb->plugin_instance = plugin_instance; - pb->plugin_instance_name = plugin_instance_name; + mjmPlugin* plugin = &pb->plugin; + plugin->active = true; + plugin->instance = (mjElement)plugin_instance; + mjm_setString(plugin->name, plugin_name.c_str()); + mjm_setString(plugin->instance_name, plugin_instance_name.c_str()); } } } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 45201210..524f2365 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -39,7 +39,7 @@ typedef enum _mjtFcompType { class mjCFlexcomp { public: mjCFlexcomp(void); - bool Make(mjCModel* model, mjCBody* body, char* error, int error_sz); + bool Make(mjCModel* model, mjmBody* body, char* error, int error_sz); bool MakeGrid(char* error, int error_sz); bool MakeBox(char* error, int error_sz); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index a6373022..e50cf1f0 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -26,6 +26,8 @@ #include #include +#include "user/user_api.h" + #ifdef MUJOCO_TINYOBJLOADER_IMPL #define TINYOBJLOADER_IMPLEMENTATION #endif @@ -176,6 +178,10 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); + + // point to local (needs to be after defaults) + plugin.name = (mjString)&plugin_name; + plugin.instance_name = (mjString)&plugin_instance_name; } @@ -302,26 +308,27 @@ void mjCMesh::LoadSDF() { name.c_str(), id); } + mjCPlugin* plugin_instance = (mjCPlugin*)plugin.instance; model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance); - const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); - if (!(plugin->capabilityflags & mjPLUGIN_SDF)) { - throw mjCError(this, "plugin '%s' does not support signed distance fields", plugin->name); + const mjpPlugin* pplugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); + if (!(pplugin->capabilityflags & mjPLUGIN_SDF)) { + throw mjCError(this, "plugin '%s' does not support signed distance fields", pplugin->name); } - std::vector attributes(plugin->nattribute, 0); - std::vector names(plugin->nattribute, 0); - std::vector values(plugin->nattribute, 0); - for (int i=0; i < plugin->nattribute; i++) { - names[i] = plugin->attributes[i]; + std::vector attributes(pplugin->nattribute, 0); + std::vector names(pplugin->nattribute, 0); + std::vector values(pplugin->nattribute, 0); + for (int i=0; i < pplugin->nattribute; i++) { + names[i] = pplugin->attributes[i]; values[i] = plugin_instance->config_attribs[names[i]].c_str(); } - if (plugin->sdf_attribute) { - plugin->sdf_attribute(attributes.data(), names.data(), values.data()); + if (pplugin->sdf_attribute) { + pplugin->sdf_attribute(attributes.data(), names.data(), values.data()); } mjtNum aabb[6] = {0}; - plugin->sdf_aabb(aabb, attributes.data()); + pplugin->sdf_aabb(aabb, attributes.data()); mjtNum total = aabb[3] + aabb[4] + aabb[5]; const mjtNum n = 300; @@ -337,7 +344,7 @@ void mjCMesh::LoadSDF() { mjtNum point[] = {aabb[0]-aabb[3] + 2 * aabb[3] * i / (nx-1), aabb[1]-aabb[4] + 2 * aabb[4] * j / (ny-1), aabb[2]-aabb[5] + 2 * aabb[5] * k / (nz-1)}; - field[(k * ny + j) * nx + i] = plugin->sdf_staticdistance(point, attributes.data()); + field[(k * ny + j) * nx + i] = pplugin->sdf_staticdistance(point, attributes.data()); } } } @@ -410,7 +417,7 @@ void mjCMesh::Compile(const mjVFS* vfs) { } // create using marching cubes - else if (is_plugin) { + else if (plugin.active) { LoadSDF(); } @@ -2444,9 +2451,10 @@ void mjCFlex::Compile(const mjVFS* vfs) { mjXUtil::Vector2String(useredge, edgeidx); for (int i=0; i<(int)vertbodyid.size(); i++) { - if (model->bodies[vertbodyid[i]]->plugin_instance) { - model->bodies[vertbodyid[i]]->plugin_instance->config_attribs["face"] = userface; - model->bodies[vertbodyid[i]]->plugin_instance->config_attribs["edge"] = useredge; + if (model->bodies[vertbodyid[i]]->plugin.instance) { + mjCPlugin* plugin_instance = (mjCPlugin*)model->bodies[vertbodyid[i]]->plugin.instance; + plugin_instance->config_attribs["face"] = userface; + plugin_instance->config_attribs["edge"] = useredge; } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 1827ed87..e627426a 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1491,7 +1491,7 @@ void mjCModel::CopyTree(mjModel* m) { m->body_mass[i] = (mjtNum)pb->mass; copyvec(m->body_inertia+3*i, pb->inertia, 3); m->body_gravcomp[i] = pb->gravcomp; - copyvec(m->body_user+nuser_body*i, pb->userdata.data(), nuser_body); + copyvec(m->body_user+nuser_body*i, pb->get_userdata().data(), nuser_body); m->body_contype[i] = pb->contype; m->body_conaffinity[i] = pb->conaffinity; @@ -2883,7 +2883,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { if (nuser_body == -1) { nuser_body = 0; for (int i=0; iuserdata.size()); + nuser_body = mjMAX(nuser_body, bodies[i]->spec_userdata_.size()); } } if (nuser_jnt == -1) { @@ -3050,8 +3050,8 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { // set actuator_plugin to the plugin instance ID std::vector> plugin_to_actuators(nplugin); for (int i = 0; i < nu; ++i) { - if (actuators[i]->is_plugin) { - int actuator_plugin = actuators[i]->plugin_instance->id; + if (actuators[i]->plugin.active) { + int actuator_plugin = ((mjCPlugin*)actuators[i]->plugin.instance)->id; m->actuator_plugin[i] = actuator_plugin; plugin_to_actuators[actuator_plugin].push_back(i); } else { @@ -3060,16 +3060,16 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { } for (int i = 0; i < nbody; ++i) { - if (bodies[i]->is_plugin) { - m->body_plugin[i] = bodies[i]->plugin_instance->id; + if (bodies[i]->plugin.active) { + m->body_plugin[i] = ((mjCPlugin*)bodies[i]->plugin.instance)->id; } else { m->body_plugin[i] = -1; } } for (int i = 0; i < ngeom; ++i) { - if (geoms[i]->is_plugin) { - m->geom_plugin[i] = geoms[i]->plugin_instance->id; + if (geoms[i]->plugin.active) { + m->geom_plugin[i] = ((mjCPlugin*)geoms[i]->plugin.instance)->id; } else { m->geom_plugin[i] = -1; } @@ -3078,7 +3078,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { std::vector> plugin_to_sensors(nplugin); for (int i = 0; i < nsensor; ++i) { if (sensors[i]->type == mjSENS_PLUGIN) { - int sensor_plugin = sensors[i]->plugin_instance->id; + int sensor_plugin = ((mjCPlugin*)sensors[i]->plugin.instance)->id; m->sensor_plugin[i] = sensor_plugin; plugin_to_sensors[sensor_plugin].push_back(i); } else { @@ -3321,7 +3321,7 @@ bool mjCModel::CopyBack(const mjModel* m) { copyvec(pb->inertia, m->body_inertia+3*i, 3); if (nuser_body) { - copyvec(pb->userdata.data(), m->body_user + nuser_body*i, nuser_body); + copyvec(pb->userdata_.data(), m->body_user + nuser_body*i, nuser_body); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 1b54b1ca..ee8f9a4d 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -493,10 +493,12 @@ mjCBase::mjCBase() { frame = nullptr; // plugin variables - is_plugin = false; - plugin_instance = nullptr; + plugin.active = false; + plugin.instance = nullptr; plugin_name = ""; plugin_instance_name = ""; + plugin.name = (mjString)&plugin_name; + plugin.instance_name = (mjString)&plugin_instance_name; } @@ -550,26 +552,32 @@ mjCBody::mjCBody(mjCModel* _model) { // set model pointer model = _model; - // missing information, must be supplied later - pos[0] = ipos[0] = mjNAN; + spec.pos[0] = spec.ipos[0] = mjNAN; // clear variables - fullinertia[0] = mjNAN; - explicitinertial = false; - mocap = false; - mjuu_setvec(quat, 1, 0, 0, 0); - mjuu_setvec(iquat, 1, 0, 0, 0); - mjuu_zerovec(pos+1, 2); - mjuu_zerovec(ipos+1, 2); - mass = 0; - mjuu_setvec(inertia, 0, 0, 0); + spec.fullinertia[0] = mjNAN; + spec.explicitinertial = false; + spec.mocap = false; + mjuu_setvec(spec.quat, 1, 0, 0, 0); + mjuu_setvec(spec.iquat, 1, 0, 0, 0); + mjuu_zerovec(spec.pos+1, 2); + mjuu_zerovec(spec.ipos+1, 2); + spec.mass = 0; + mjuu_setvec(spec.inertia, 0, 0, 0); parentid = -1; weldid = -1; dofnum = 0; lastdof = -1; subtreedofs = 0; - gravcomp = 0; - userdata.clear(); + spec.gravcomp = 0; + spec_userdata_.clear(); + spec.alt.axisangle[0] = spec.alt.xyaxes[0] = spec.alt.zaxis[0] = + spec.alt.euler[0] = mjNAN; + spec.ialt.axisangle[0] = spec.ialt.xyaxes[0] = spec.ialt.zaxis[0] = + spec.ialt.euler[0] = mjNAN; + + spec.plugin.active = false; + spec.plugin.instance = nullptr; contype = 0; conaffinity = 0; @@ -585,6 +593,37 @@ mjCBody::mjCBody(mjCModel* _model) { sites.clear(); cameras.clear(); lights.clear(); + + // point to local + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.userdata = (mjDouble)&spec_userdata_; + spec.plugin.name = (mjString)&plugin_name; + spec.plugin.instance_name = (mjString)&plugin_instance_name; + + // in case this body is not compiled + CopyFromSpec(); +} + + +void mjCBody::CopyFromSpec() { + *static_cast(this) = spec; + userdata_ = spec_userdata_; + userdata = (mjDouble)&userdata_; + spec.info = (mjString)&info; + mju_copy4(alt_.axisangle, alt.axisangle); + mju_copy(alt_.xyaxes, alt.xyaxes, 6); + mju_copy3(alt_.zaxis, alt.zaxis); + mju_copy3(alt_.euler, alt.euler); + mju_copy4(ialt_.axisangle, ialt.axisangle); + mju_copy(ialt_.xyaxes, ialt.xyaxes, 6); + mju_copy3(ialt_.zaxis, ialt.zaxis); + mju_copy3(ialt_.euler, ialt.euler); + plugin.active = spec.plugin.active; + plugin.instance = spec.plugin.instance; + plugin.name = spec.plugin.name; + plugin.instance_name = spec.plugin.instance_name; } @@ -934,18 +973,20 @@ const char* mjCBody::FullInertia(double quat[4], double inertia[3]) { // set explicitinertial to true void mjCBody::MakeInertialExplicit() { - explicitinertial = true; + spec.explicitinertial = true; } // compiler void mjCBody::Compile(void) { + CopyFromSpec(); + // resize userdata - if (userdata.size() > model->nuser_body) { + if (userdata_.size() > model->nuser_body) { throw mjCError(this, "user has more values than nuser_body in body '%s' (id = %d)", name.c_str(), id); } - userdata.resize(model->nuser_body); + userdata_.resize(model->nuser_body); // pos defaults to (0,0,0) if (!mjuu_defined(pos[0])) { @@ -963,7 +1004,7 @@ void mjCBody::Compile(void) { } // check and process orientation alternatives for body - const char* err = alt.Set(quat, model->degree, model->euler); + const char* err = alt_.Set(quat, model->degree, model->euler); if (err) { throw mjCError(this, "error '%s' in frame alternative", err); } @@ -1101,17 +1142,18 @@ void mjCBody::Compile(void) { for (int i=0; iCompile(); // plugin - if (is_plugin) { + if (plugin.active) { if (plugin_name.empty() && plugin_instance_name.empty()) { throw mjCError( this, "neither 'plugin' nor 'instance' is specified for body '%s', (id = %d)", name.c_str(), id); } - model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance); - const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); - if (!(plugin->capabilityflags & mjPLUGIN_PASSIVE)) { - throw mjCError(this, "plugin '%s' does not support passive forces", plugin->name); + mjCPlugin** plugin_instance = (mjCPlugin**)&plugin.instance; + model->ResolvePlugin(this, plugin_name, plugin_instance_name, plugin_instance); + const mjpPlugin* pplugin = mjp_getPluginAtSlot((*plugin_instance)->plugin_slot); + if (!(pplugin->capabilityflags & mjPLUGIN_PASSIVE)) { + throw mjCError(this, "plugin '%s' does not support passive forces", pplugin->name); } } @@ -1399,11 +1441,9 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) { model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); - // plugin variables - is_plugin = false; - plugin_instance = nullptr; - plugin_name = ""; - plugin_instance_name = ""; + // point to local (needs to be after defaults) + plugin.name = (mjString)&plugin_name; + plugin.instance_name = (mjString)&plugin_instance_name; } @@ -1915,17 +1955,18 @@ void mjCGeom::Compile(void) { } // plugin - if (is_plugin) { + if (plugin.active) { if (plugin_name.empty() && plugin_instance_name.empty()) { throw mjCError( this, "neither 'plugin' nor 'instance' is specified for geom '%s', (id = %d)", name.c_str(), id); } - model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance); - const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); - if (!(plugin->capabilityflags & mjPLUGIN_SDF)) { - throw mjCError(this, "plugin '%s' does not support sign distance fields", plugin->name); + mjCPlugin** plugin_instance = (mjCPlugin**)&plugin.instance; + model->ResolvePlugin(this, plugin_name, plugin_instance_name, plugin_instance); + const mjpPlugin* pplugin = mjp_getPluginAtSlot((*plugin_instance)->plugin_slot); + if (!(pplugin->capabilityflags & mjPLUGIN_SDF)) { + throw mjCError(this, "plugin '%s' does not support sign distance fields", pplugin->name); } } @@ -3921,10 +3962,9 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); - is_plugin = false; - plugin_instance = nullptr; - plugin_name = ""; - plugin_instance_name = ""; + // point to local (needs to be after defaults) + plugin.name = (mjString)&plugin_name; + plugin.instance_name = (mjString)&plugin_instance_name; } @@ -4106,17 +4146,18 @@ void mjCActuator::Compile(void) { } // plugin - if (is_plugin) { + if (plugin.active) { if (plugin_name.empty() && plugin_instance_name.empty()) { throw mjCError( this, "neither 'plugin' nor 'instance' is specified for actuator '%s', (id = %d)", name.c_str(), id); } - model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance); - const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); - if (!(plugin->capabilityflags & mjPLUGIN_ACTUATOR)) { - throw mjCError(this, "plugin '%s' does not support actuators", plugin->name); + mjCPlugin** plugin_instance = (mjCPlugin**)&plugin.instance; + model->ResolvePlugin(this, plugin_name, plugin_instance_name, plugin_instance); + const mjpPlugin* pplugin = mjp_getPluginAtSlot((*plugin_instance)->plugin_slot); + if (!(pplugin->capabilityflags & mjPLUGIN_ACTUATOR)) { + throw mjCError(this, "plugin '%s' does not support actuators", pplugin->name); } } } @@ -4147,9 +4188,9 @@ mjCSensor::mjCSensor(mjCModel* _model) { obj = nullptr; refid = -1; - plugin_instance = nullptr; - plugin_name = ""; - plugin_instance_name = ""; + // point to local (needs to be after defaults) + plugin.name = (mjString)&plugin_name; + plugin.instance_name = (mjString)&plugin_instance_name; } @@ -4520,12 +4561,13 @@ void mjCSensor::Compile(void) { // resolve plugin instance, or create one if using the "plugin" attribute shortcut { - model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance); - const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot); - if (!(plugin->capabilityflags & mjPLUGIN_SENSOR)) { - throw mjCError(this, "plugin '%s' does not support sensors", plugin->name); + mjCPlugin** plugin_instance = (mjCPlugin**)&plugin.instance; + model->ResolvePlugin(this, plugin_name, plugin_instance_name, plugin_instance); + const mjpPlugin* pplugin = mjp_getPluginAtSlot((*plugin_instance)->plugin_slot); + if (!(pplugin->capabilityflags & mjPLUGIN_SENSOR)) { + throw mjCError(this, "plugin '%s' does not support sensors", pplugin->name); } - needstage = static_cast(plugin->needstage); + needstage = static_cast(pplugin->needstage); } break; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index ac2e5247..98a323a9 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -173,7 +173,6 @@ class mjCBoundingVolumeHierarchy { //------------------------- class mjCBase ---------------------------------------------------------- // Generic functionality for all derived classes -class mjCPlugin; class mjCBase { friend class mjCDef; @@ -197,11 +196,9 @@ class mjCBase { mjCFrame* frame; // pointer to frame transformation // plugin support - bool is_plugin; + mjmPlugin plugin; std::string plugin_name; std::string plugin_instance_name; - mjCPlugin* plugin_instance; - protected: mjCBase(); // constructor }; @@ -211,13 +208,15 @@ class mjCBase { //------------------------- class mjCBody ----------------------------------------------- // Describes a rigid body -class mjCBody : public mjCBase { +class mjCBody : public mjCBase, private mjmBody { friend class mjCJoint; friend class mjCGeom; friend class mjCSite; friend class mjCCamera; + friend class mjCComposite; friend class mjCLight; friend class mjCFlex; + friend class mjCFlexcomp; friend class mjCEquality; friend class mjCPair; friend class mjCModel; @@ -248,33 +247,32 @@ class mjCBody : public mjCBase { // return nullptr on success, error string on failure const char* FullInertia(double quat[4], double inertia[3]); - // variables set by user or 'Compile' - bool mocap; // is this a mocap body - double pos[3]; // frame position - double quat[4]; // frame orientation - double ipos[3]; // inertial frame position - double iquat[4]; // inertial frame orientation - double mass; // mass - double inertia[3]; // diagonal inertia (in i-frame) - double gravcomp; // gravity compensation - std::vector userdata; // user data - double fullinertia[6]; // non-axis-aligned inertia matrix - mjCAlternative alt; // alternative orientation specification - mjCAlternative ialt; // alternative for inertial frame + // variables set by user + mjmBody spec; + + // inherited + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; + using mjCBase::plugin; + + // used by mjXWriter and mjCModel + const std::vector& get_userdata() { return userdata_; } + + mjCAlternative alt_; + mjCAlternative ialt_; // variables computed by 'Compile' and 'AddXXX' private: mjCBody(mjCModel*); // constructor ~mjCBody(); // destructor - void Compile(void); // compiler - + void Compile(void); // compiler void GeomFrame(void); // get inertial info from geoms int parentid; // parent index in global array int weldid; // top index of body we are welded to int dofnum; // number of motion dofs for body int mocapid; // mocap id, -1: not mocap - bool explicitinertial; // whether to save the body with an explicit inertial clause int contype; // OR over geom contypes int conaffinity; // OR over geom conaffinities @@ -296,6 +294,12 @@ class mjCBody : public mjCBase { std::vector sites; // sites attached to this body std::vector cameras; // cameras attached to this body std::vector lights; // lights attached to this body + + void CopyFromSpec(); // copy spec into attributes + + // variable-size data + std::vector userdata_; + std::vector spec_userdata_; }; @@ -482,7 +486,6 @@ class mjCSite : public mjCBase, private mjmSite { void CopyFromSpec(); // copy spec into attributes mjCAlternative alt_; - mjCAlternative spec_alt_; // variable-size data std::string material_; @@ -1223,11 +1226,6 @@ class mjCSensor : public mjCBase { double noise; // noise stdev std::vector userdata; // user data - // plugin support - std::string plugin_name; - std::string plugin_instance_name; - mjCPlugin* plugin_instance; - private: mjCSensor(mjCModel*); // constructor void Compile(void); // compiler diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index baa00745..67c947f2 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -873,7 +873,7 @@ void mjXReader::Parse(XMLElement* root) { for (XMLElement* section = root->FirstChildElement("worldbody"); section; section = section->NextSiblingElement("worldbody")) { - Body(section, model->GetWorld(), nullptr); + Body(section, &model->GetWorld()->spec, nullptr); } for (XMLElement* section = root->FirstChildElement("contact"); section; @@ -1350,7 +1350,7 @@ void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) { XMLElement* eplugin = elem->FirstChildElement("plugin"); if (eplugin) { - OnePlugin(eplugin, pmesh); + OnePlugin(eplugin, &pmesh->plugin); } if (MapValue(elem, "smoothnormal", &n, bool_map, 2)) { @@ -1542,7 +1542,7 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) { // plugin sub-element XMLElement* eplugin = elem->FirstChildElement("plugin"); if (eplugin) { - OnePlugin(eplugin, pgeom); + OnePlugin(eplugin, &pgeom->plugin); } // remaining attributes @@ -2044,7 +2044,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { } else if (type == "plugin") { - OnePlugin(elem, pact); + OnePlugin(elem, &pact->plugin); int n; if (MapValue(elem, "dyntype", &n, dyn_map, dyn_sz)) { pact->dyntype = (mjtDyn)n; @@ -2068,7 +2068,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { // make composite -void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) { +void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) { string text; int n; @@ -2091,7 +2091,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) { ReadAttrTxt(eplugin, "plugin", comp.plugin_name); ReadAttrTxt(eplugin, "instance", comp.plugin_instance_name); if (comp.plugin_instance_name.empty()) { - comp.plugin_instance = model->AddPlugin(); + comp.plugin_instance = (mjCPlugin*)mjm_addPlugin(model); comp.plugin_instance->name = "composite"+comp.prefix; comp.plugin_instance_name = comp.plugin_instance->name; } else { @@ -2286,7 +2286,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) { // make composite char error[200]; - bool res = comp.Make(pbody->model, pbody, error, 200); + bool res = comp.Make((mjCModel*)mjm_getModel(pbody), pbody, error, 200); // throw error if (!res) { @@ -2297,7 +2297,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) { // make flexcomp -void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) { +void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) { string text; int n; @@ -2407,7 +2407,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) { ReadAttrTxt(eplugin, "plugin", fcomp.plugin_name); ReadAttrTxt(eplugin, "instance", fcomp.plugin_instance_name); if (fcomp.plugin_instance_name.empty()) { - fcomp.plugin_instance = model->AddPlugin(); + fcomp.plugin_instance = (mjCPlugin*)mjm_addPlugin(model); fcomp.plugin_instance->name = "flexcomp_" + fcomp.name; fcomp.plugin_instance_name = fcomp.plugin_instance->name; } else { @@ -2418,7 +2418,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) { // make flexcomp char error[200]; - bool res = fcomp.Make(pbody->model, pbody, error, 200); + bool res = fcomp.Make((mjCModel*)mjm_getModel(pbody), pbody, error, 200); // throw error if (!res) { @@ -2429,13 +2429,17 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) { // add plugin -void mjXReader::OnePlugin(XMLElement* elem, mjCBase* object) { - object->is_plugin = true; - ReadAttrTxt(elem, "plugin", object->plugin_name); - ReadAttrTxt(elem, "instance", object->plugin_instance_name); - if (object->plugin_instance_name.empty()) { - object->plugin_instance = model->AddPlugin(); - ReadPluginConfigs(elem, object->plugin_instance); +void mjXReader::OnePlugin(XMLElement* elem, mjmPlugin* plugin) { + plugin->active = true; + std::string name = ""; + std::string instance_name = ""; + ReadAttrTxt(elem, "plugin", name); + ReadAttrTxt(elem, "instance", instance_name); + mjm_setString(plugin->name, name.c_str()); + mjm_setString(plugin->instance_name, instance_name.c_str()); + if (instance_name.empty()) { + plugin->instance = mjm_addPlugin(model); + ReadPluginConfigs(elem, (mjCPlugin*)plugin->instance); } else { model->hasImplicitPluginElem = true; } @@ -2579,7 +2583,7 @@ void mjXReader::Extension(XMLElement* section) { throw mjXError( child, "explicit plugin instance must appear before implicit plugin elements"); } - mjCPlugin* pp = model->AddPlugin(); + mjCPlugin* pp = (mjCPlugin*)mjm_addPlugin(model); GetXMLPos(child, pp); ReadAttrTxt(child, "name", pp->name, /* required = */ true); if (pp->name.empty()) { @@ -2983,7 +2987,7 @@ void mjXReader::Asset(XMLElement* section) { // body/world section parser; recursive -void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { +void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjCFrame* frame) { string text, name; XMLElement* elem; int n; @@ -2994,7 +2998,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { } // no attributes allowed in world body - if (pbody->id==0 && section->FirstAttribute() && !frame) { + if (mjm_getId(pbody->element)==0 && section->FirstAttribute() && !frame) { throw mjXError(section, "World body cannot have attributes"); } @@ -3007,13 +3011,13 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { // get class if specified, otherwise use body mjCDef* def = GetClass(elem); if (!def) { - def = pbody->def; + def = (mjCDef*)mjm_getDefault(pbody->element); } // inertial sub-element if (name=="inertial") { // no inertia allowed in world body - if (pbody->id==0) { + if (mjm_getId(pbody->element)==0) { throw mjXError(elem, "World body cannot have inertia"); } pbody->explicitinertial = true; @@ -3031,12 +3035,12 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { // joint sub-element else if (name=="joint") { // no joints allowed in world body - if (pbody->id==0) { + if (mjm_getId(pbody->element)==0) { throw mjXError(elem, "World body cannot have joints"); } // create joint and parse - mjCJoint* pjoint = pbody->AddJoint(def); + mjCJoint* pjoint = (mjCJoint*)mjm_addJoint(pbody, def); OneJoint(elem, pjoint); pjoint->SetFrame(frame); } @@ -3044,12 +3048,12 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { // freejoint sub-element else if (name=="freejoint") { // no joints allowed in world body - if (pbody->id==0) { + if (mjm_getId(pbody->element)==0) { throw mjXError(elem, "World body cannot have joints"); } // create free joint without defaults - mjCJoint* pjoint = pbody->AddFreeJoint(); + mjCJoint* pjoint = (mjCJoint*)mjm_addFreeJoint(pbody); pjoint->SetFrame(frame); // save defaults after creation, to make sure writing is ok @@ -3063,7 +3067,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { // geom sub-element else if (name=="geom") { // create geom and parse - mjCGeom* pgeom = pbody->AddGeom(def); + mjCGeom* pgeom = (mjCGeom*)mjm_addGeom(pbody, def); OneGeom(elem, pgeom); pgeom->SetFrame(frame); } @@ -3073,13 +3077,13 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { // create site and parse mjmSite* site = mjm_addSite(pbody, def); OneSite(elem, *site); - mjm_setFrame(site, frame); + mjm_setFrame(site->element, frame); } // camera sub-element else if (name=="camera") { // create camera and parse - mjCCamera* pcam = pbody->AddCamera(def); + mjCCamera* pcam = (mjCCamera*)mjm_addCamera(pbody, def); OneCamera(elem, pcam); pcam->SetFrame(frame); } @@ -3087,14 +3091,14 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { // light sub-element else if (name=="light") { // create light and parse - mjCLight* plight = pbody->AddLight(def); + mjCLight* plight = (mjCLight*)mjm_addLight(pbody, def); OneLight(elem, plight); plight->SetFrame(frame); } // plugin sub-element else if (name == "plugin") { - OnePlugin(elem, pbody); + OnePlugin(elem, &(pbody->plugin)); } // composite sub-element @@ -3111,7 +3115,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { // frame sub-element else if (name=="frame") { - mjCFrame* pframe = pbody->AddFrame(frame); + mjCFrame* pframe = (mjCFrame*)mjm_addFrame(pbody, frame); GetXMLPos(elem, pframe); ReadAttr(elem, "pos", 3, pframe->pos, text); @@ -3133,12 +3137,16 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { } // create child body - mjCBody* pchild = pbody->AddBody(childdef); - GetXMLPos(elem, pchild); + mjmBody* pchild = mjm_addBody(pbody, childdef); + mjm_setString(pchild->info, + std::string("line = " + std::to_string(elem->GetLineNum())).c_str()); // read attributes - ReadAttrTxt(elem, "name", pchild->name); - ReadAttrTxt(elem, "childclass", pchild->classname); + std::string name, childclass; + ReadAttrTxt(elem, "name", name); + mjm_setString(pchild->name, name.c_str()); + ReadAttrTxt(elem, "childclass", childclass); + mjm_setString(pchild->classname, childclass.c_str()); ReadAttr(elem, "pos", 3, pchild->pos, text); ReadQuat(elem, "quat", pchild->quat, text); if (MapValue(elem, "mocap", &n, bool_map, 2)) { @@ -3150,10 +3158,12 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { ReadAttr(elem, "gravcomp", 1, &pchild->gravcomp, text); // read userdata - ReadVector(elem, "user", pchild->userdata, text); + std::vector userdata; + ReadVector(elem, "user", userdata, text); + mjm_setDouble(pchild->userdata, userdata.data(), userdata.size()); // add frame - pchild->SetFrame(frame); + mjm_setFrame(pchild->element, frame); // make recursive call Body(elem, pchild, nullptr); @@ -3373,6 +3383,8 @@ void mjXReader::Sensor(XMLElement* section) { // create sensor, get string type mjCSensor* psen = model->AddSensor(); string type = elem->Value(); + string plugin_name = ""; + string instance_name = ""; // read name, noise, userdata ReadAttrTxt(elem, "name", psen->name); @@ -3630,14 +3642,16 @@ void mjXReader::Sensor(XMLElement* section) { else if (type=="plugin") { psen->type = mjSENS_PLUGIN; - ReadAttrTxt(elem, "plugin", psen->plugin_name); - ReadAttrTxt(elem, "instance", psen->plugin_instance_name); - if (psen->plugin_instance_name.empty()) { - psen->plugin_instance = model->AddPlugin(); + 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, psen->plugin_instance); + ReadPluginConfigs(elem, (mjCPlugin*)psen->plugin.instance); ReadAttrTxt(elem, "objtype", text); psen->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", psen->objname); diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 6ccf5e36..583ebace 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -42,7 +42,7 @@ class mjXReader : public mjXBase { void Visual(tinyxml2::XMLElement* section); // visual section void Statistic(tinyxml2::XMLElement* section); // statistic section void Asset(tinyxml2::XMLElement* section); // asset section - void Body(tinyxml2::XMLElement* section, mjCBody* pbody, + void Body(tinyxml2::XMLElement* section, mjmBody* pbody, mjCFrame* pframe); // body/world section void Contact(tinyxml2::XMLElement* section); // contact section void Deformable(tinyxml2::XMLElement* section); // deformable section @@ -66,9 +66,9 @@ class mjXReader : public mjXBase { void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality); void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon); void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator); - void OneComposite(tinyxml2::XMLElement* elem, mjCBody* pbody, mjCDef* def); - void OneFlexcomp(tinyxml2::XMLElement* elem, mjCBody* pbody); - void OnePlugin(tinyxml2::XMLElement* elem, mjCBase* object); + void OneComposite(tinyxml2::XMLElement* elem, mjmBody* pbody, mjCDef* def); + void OneFlexcomp(tinyxml2::XMLElement* elem, mjmBody* pbody); + void OnePlugin(tinyxml2::XMLElement* elem, mjmPlugin* plugin); mjXSchema schema; // schema used for validation mjCDef* GetClass(tinyxml2::XMLElement* section); // get default class name diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 98f9f793..c30ff289 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -26,6 +26,7 @@ #include "engine/engine_plugin.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" +#include "user/user_api.h" #include "user/user_model.h" #include "user/user_objects.h" #include "user/user_util.h" @@ -417,7 +418,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) { } // write plugin - if (pgeom->is_plugin) { + if (pgeom->plugin.active) { OnePlugin(InsertEnd(elem, "plugin"), pgeom); } } @@ -725,7 +726,7 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm); // plugins: write config attributes - if (pact->is_plugin) { + if (pact->plugin.active) { OnePlugin(elem, pact); } @@ -756,13 +757,15 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { // write plugin void mjXWriter::OnePlugin(XMLElement* elem, mjCBase* object) { - if (!object->plugin_instance_name.empty()) { - WriteAttrTxt(elem, "instance", object->plugin_instance_name); + const std::string instance_name = std::string(mjm_getString(object->plugin.instance_name)); + const std::string plugin_name = std::string(mjm_getString(object->plugin.name)); + if (!instance_name.empty()) { + WriteAttrTxt(elem, "instance", instance_name); } else { - WriteAttrTxt(elem, "plugin", object->plugin_name); + WriteAttrTxt(elem, "plugin", plugin_name); const mjpPlugin* plugin = mjp_getPluginAtSlot( - object->plugin_instance->plugin_slot); - const char* c = &object->plugin_instance->flattened_attributes[0]; + ((mjCPlugin*)object->plugin.instance)->plugin_slot); + const char* c = &((mjCPlugin*)object->plugin.instance)->flattened_attributes[0]; for (int i = 0; i < plugin->nattribute; ++i) { std::string value(c); if (!value.empty()) { @@ -1424,7 +1427,7 @@ void mjXWriter::Asset(XMLElement* root) { for (int i=0; iGetObject(mjOBJ_MESH, i); - if (pmesh->is_plugin) { + if (pmesh->plugin.active) { elem = InsertEnd(section, "mesh"); WriteAttrTxt(elem, "name", pmesh->name); OnePlugin(InsertEnd(elem, "plugin"), pmesh); @@ -1487,7 +1490,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { WriteAttr(elem, "gravcomp", 1, &body->gravcomp); } // userdata - WriteVector(elem, "user", body->userdata); + WriteVector(elem, "user", body->get_userdata()); // write inertial if (body->explicitinertial && @@ -1526,7 +1529,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { } // write plugin - if (body->is_plugin) { + if (body->plugin.active) { OnePlugin(InsertEnd(elem, "plugin"), body); } @@ -1709,7 +1712,7 @@ void mjXWriter::Actuator(XMLElement* root) { for (int i=0; iGetObject(mjOBJ_ACTUATOR, i); XMLElement* elem; - if (pact->is_plugin) { + if (pact->plugin.active) { elem = InsertEnd(section, "plugin"); } else { elem = InsertEnd(section, "general"); @@ -1737,6 +1740,8 @@ void mjXWriter::Sensor(XMLElement* root) { for (int i=0; isensors[i]; + std::string instance_name = ""; + std::string plugin_name = ""; // write sensor type and type-specific attributes switch (psen->type) { @@ -1923,13 +1928,15 @@ void mjXWriter::Sensor(XMLElement* root) { WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); WriteAttrTxt(elem, "objname", psen->objname); } - if (!psen->plugin_instance_name.empty()) { - WriteAttrTxt(elem, "instance", psen->plugin_instance_name); + 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", psen->plugin_name); + WriteAttrTxt(elem, "plugin", plugin_name); const mjpPlugin* plugin = mjp_getPluginAtSlot( - psen->plugin_instance->plugin_slot); - const char* c = &psen->plugin_instance->flattened_attributes[0]; + ((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()) { diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index d1cae5db..8e9197be 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -18,6 +18,7 @@ #include #include +#include "user/user_api.h" #include "user/user_model.h" #include "user/user_objects.h" #include "user/user_util.h" @@ -201,14 +202,15 @@ void mjXURDF::Parse( // override the pose for the base link and add a free joint for (int i = 0; i < (int)urName.size(); i++) { if (urParent[i] < 0) { - mjCBody* pbody = (mjCBody*)model->GetWorld()->FindObject(mjOBJ_BODY, urName[i]); + mjmBody* world = mjm_findBody(model, "world"); + mjmBody* pbody = mjm_findChild(world, urName[i].c_str()); mjuu_copyvec(pbody->pos, pos, 3); mjuu_copyvec(pbody->quat, quat, 4); // add a free joint to allow motion of the body // if the mass is 0, assume the object is static if (!static_body && pbody->mass > 0) { - auto pjoint = pbody->AddJoint(); + mjCJoint* pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); pjoint->name = urName[i] + "_free_joint"; pjoint->type = mjJNT_FREE; } @@ -220,13 +222,14 @@ void mjXURDF::Parse( void mjXURDF::Body(XMLElement* body_elem) { std::string name, text; XMLElement *elem, *temp, *temp1; - mjCBody* pbody; + mjmBody *pbody, *world; mjCGeom* pgeom; - // get body name and pointer to mjCBody + // get body name and pointer to mjmBody ReadAttrTxt(body_elem, "name", name, true); name = GetPrefixedName(name); - pbody = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name); + world = mjm_findBody(model, "world"); + pbody = mjm_findChild(world, name.c_str()); if (!pbody) { throw mjXError(body_elem, "URDF body not found"); // SHOULD NOT OCCUR } @@ -264,7 +267,7 @@ void mjXURDF::Body(XMLElement* body_elem) { // process inertia // lquat = rotation from specified to default (joint/body) inertial frame double lquat[4], tmpquat[4]; - const char* altres = pbody->FullInertia(lquat, pbody->inertia); + const char* altres = mjm_setFullInertia(pbody, lquat, pbody->inertia); // inertia are sometimes 0 in URDF files: ignore error in altres, fix later (void) altres; @@ -366,7 +369,7 @@ void mjXURDF::Parse(XMLElement* root) { void mjXURDF::Joint(XMLElement* joint_elem) { std::string jntname, name, text; XMLElement *elem; - mjCBody *pbody, *parent; + mjmBody *pbody, *parent, *world; mjCJoint *pjoint=0, *pjoint1=0, *pjoint2=0; int jointtype; @@ -382,7 +385,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) { elem = FindSubElem(joint_elem, "parent", true); ReadAttrTxt(elem, "link", name, true); name = GetPrefixedName(name); - parent = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name); + world = mjm_findBody(model, "world"); + parent = mjm_findChild(world, name.c_str()); if (!parent) { // SHOULD NOT OCCUR throw mjXError(elem, "invalid parent name in URDF joint definition"); } @@ -391,7 +395,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) { elem = FindSubElem(joint_elem, "child", true); ReadAttrTxt(elem, "link", name, true); name = GetPrefixedName(name); - pbody = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, name); + world = mjm_findBody(model, "world"); + pbody = mjm_findChild(world, name.c_str()); if (!pbody) { // SHOULD NOT OCCUR throw mjXError(elem, "invalid child name in URDF joint definition"); } @@ -408,7 +413,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { switch (jointtype) { case 0: // revolute case 1: // continuous - pjoint = pbody->AddJoint(); + pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); pjoint->name = jntname; pjoint->type = mjJNT_HINGE; mjuu_setvec(pjoint->pos, 0, 0, 0); @@ -416,7 +421,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { break; case 2: // prismatic - pjoint = pbody->AddJoint(); + pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); pjoint->name = jntname; pjoint->type = mjJNT_SLIDE; mjuu_setvec(pjoint->pos, 0, 0, 0); @@ -427,7 +432,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { return; case 4: // floating - pjoint = pbody->AddJoint(); + pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); pjoint->name = jntname; pjoint->type = mjJNT_FREE; break; @@ -438,7 +443,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { mjuu_quat2mat(mat, quat); // construct slider along x - pjoint = pbody->AddJoint(); + pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); pjoint->name = jntname + "_TX"; pjoint->type = mjJNT_SLIDE; tmpaxis[0] = mat[0]; @@ -448,7 +453,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { mjuu_copyvec(pjoint->axis, tmpaxis, 3); // construct slider along y - pjoint1 = pbody->AddJoint(); + pjoint1 = (mjCJoint*)mjm_addJoint(pbody, 0); pjoint1->name = jntname + "_TY"; pjoint1->type = mjJNT_SLIDE; tmpaxis[0] = mat[1]; @@ -458,7 +463,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { mjuu_copyvec(pjoint1->axis, tmpaxis, 3); // construct hinge around z = locaxis - pjoint2 = pbody->AddJoint(); + pjoint2 = (mjCJoint*)mjm_addJoint(pbody, 0); pjoint2->name = jntname + "_RZ"; pjoint2->type = mjJNT_HINGE; mjuu_setvec(pjoint2->pos, 0, 0, 0); @@ -495,7 +500,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { // parse origin and geometry elements of visual or collision -mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjCBody* pbody, bool collision) { +mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjmBody* pbody, bool collision) { XMLElement *elem, *temp; std::string text, meshfile; @@ -503,7 +508,7 @@ mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjCBody* pbody, bool collision) { elem = FindSubElem(geom_elem, "geometry", true); // add BOX geom, modify type later - mjCGeom* pgeom = pbody->AddGeom(); + mjCGeom* pgeom = (mjCGeom*)mjm_addGeom(pbody, 0); pgeom->name = ""; pgeom->type = mjGEOM_BOX; if (collision) { @@ -664,21 +669,22 @@ void mjXURDF::AddBody(std::string name) { // add body with given number to the mjCModel tree, process children void mjXURDF::AddToTree(int n) { // get pointer to parent in mjCModel tree - mjCBody *parent = 0, *child = 0; + mjmBody *parent = 0, *child = 0, *world = 0; if (urParent[n]>=0) { - parent = (mjCBody*) model->GetWorld()->FindObject(mjOBJ_BODY, urName[urParent[n]]); + world = mjm_findBody(model, "world"); + parent = mjm_findChild(world, urName[urParent[n]].c_str()); if (!parent) throw mjXError(0, "URDF body parent should already be in tree: %s", urName[urParent[n]].c_str()); // SHOULD NOT OCCUR } else { - parent = model->GetWorld(); + parent = &model->GetWorld()->spec; } // add this body if (urName[n] != "world") { - child = parent->AddBody(); - child->name = urName[n]; + child = mjm_addBody(parent, 0); + mjm_setString(child->name, urName[n].c_str()); } // add children recursively diff --git a/src/xml/xml_urdf.h b/src/xml/xml_urdf.h index fdca1dfd..4fa24159 100644 --- a/src/xml/xml_urdf.h +++ b/src/xml/xml_urdf.h @@ -19,6 +19,7 @@ #include #include +#include "user/user_api.h" #include "user/user_model.h" #include "xml/xml_base.h" #include "tinyxml2.h" @@ -52,7 +53,7 @@ class mjXURDF : public mjXBase { void Body(tinyxml2::XMLElement* body_elem); // parse body void Joint(tinyxml2::XMLElement* joint_elem); // parse joint mjCGeom* Geom(tinyxml2::XMLElement* geom_elem, - mjCBody* pbody, bool collision); // parse origin and geometry of geom + mjmBody* pbody, bool collision); // parse origin and geometry of geom void Origin(tinyxml2::XMLElement* origin_elem, double* pos, double* quat); // parse origin element void MakeMaterials(tinyxml2::XMLElement* elem); // find all materials recursively diff --git a/test/engine/engine_plugin_test.cc b/test/engine/engine_plugin_test.cc index fce99165..6fc9900d 100644 --- a/test/engine/engine_plugin_test.cc +++ b/test/engine/engine_plugin_test.cc @@ -17,6 +17,7 @@ #include "src/engine/engine_plugin.h" +#include #include #include #include @@ -421,6 +422,22 @@ constexpr char xml[] = R"( )"; +TEST_F(MujocoTest, EmptyPluginDisallowed) { + static constexpr char xml[] = R"( + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, testing::IsNull()) << error.data(); + EXPECT_THAT(error.data(), HasSubstr( + "neither 'plugin' nor 'instance' is specified for body 'world'")); + mj_deleteModel(m); +} + TEST_F(PluginTest, FirstPartyPlugins) { EXPECT_THAT(mjp_pluginCount(), kNumTruePlugins); } diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 49472d5a..c8dce45e 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -18,7 +18,6 @@ #include #include #include "src/user/user_api.h" -#include #include "test/fixture.h" namespace mujoco { @@ -31,8 +30,8 @@ using ::testing::HasSubstr; TEST_F(MujocoTest, ReadWriteData) { void* model = mjm_createModel(); - void* world = mjm_findObject(model, mjOBJ_BODY, "world"); - void* body = mjm_addBody(world, 0); + mjmBody* world = mjm_findBody(model, "world"); + mjmBody* body = mjm_addBody(world, 0); mjmSite* site = mjm_addSite(body, 0); {