From e3cbad280cc33894ce2af8d2a8b9b90de5a9d1b7 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 9 Feb 2024 04:44:04 -0800 Subject: [PATCH] Add mjmJoint, mjmCamera, and mjmLight to C API. PiperOrigin-RevId: 605596008 Change-Id: Ie53ac4b99f5336380039ab5eb9b2ca4a82768b28 --- src/user/user_api.cc | 24 ++-- src/user/user_api.h | 100 +++++++++++++++- src/user/user_composite.cc | 56 ++++----- src/user/user_flexcomp.cc | 4 +- src/user/user_init.cc | 50 +++++++- src/user/user_model.cc | 14 +-- src/user/user_objects.cc | 218 +++++++++++++++++++++-------------- src/user/user_objects.h | 108 ++++++++--------- src/xml/xml_native_reader.cc | 166 ++++++++++++++++---------- src/xml/xml_native_reader.h | 6 +- src/xml/xml_native_writer.cc | 12 +- src/xml/xml_urdf.cc | 30 ++--- 12 files changed, 506 insertions(+), 282 deletions(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index a4435e42..a73bd491 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -38,6 +38,14 @@ void mjm_deleteModel(void* modelspec) { +// copy spec into private attributes +MJAPI void mjm_finalize(mjElement object) { + mjCBase* baseC = reinterpret_cast(object); + baseC->CopyFromSpec(); +} + + + // add child body to body, return child spec mjmBody* mjm_addBody(mjmBody* bodyspec, void* defspec) { mjCDef* def = static_cast(defspec); @@ -58,20 +66,20 @@ mjmSite* mjm_addSite(mjmBody* bodyspec, void* defspec) { // add joint to body -void* mjm_addJoint(mjmBody* bodyspec, void* defspec) { +mjmJoint* mjm_addJoint(mjmBody* bodyspec, void* defspec) { mjCDef* def = static_cast(defspec); mjCBody* body = reinterpret_cast(bodyspec->element); mjCJoint* joint = body->AddJoint(def); - return joint; + return &joint->spec; } // add free joint to body -void* mjm_addFreeJoint(mjmBody* bodyspec) { +mjmJoint* mjm_addFreeJoint(mjmBody* bodyspec) { mjCBody* body = reinterpret_cast(bodyspec->element); mjCJoint* joint = body->AddFreeJoint(); - return joint; + return &joint->spec; } @@ -87,21 +95,21 @@ mjmGeom* mjm_addGeom(mjmBody* bodyspec, void* defspec) { // add camera to body -void* mjm_addCamera(mjmBody* bodyspec, void* defspec) { +mjmCamera* mjm_addCamera(mjmBody* bodyspec, void* defspec) { mjCDef* def = static_cast(defspec); mjCBody* body = reinterpret_cast(bodyspec->element); mjCCamera* camera = body->AddCamera(def); - return camera; + return &camera->spec; } // add light to body -void* mjm_addLight(mjmBody* bodyspec, void* defspec) { +mjmLight* mjm_addLight(mjmBody* bodyspec, void* defspec) { mjCDef* def = static_cast(defspec); mjCBody* body = reinterpret_cast(bodyspec->element); mjCLight* light = body->AddLight(def); - return light; + return &light->spec; } diff --git a/src/user/user_api.h b/src/user/user_api.h index 3cda342b..bd429ef3 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -76,6 +76,42 @@ typedef struct _mjmBody { } mjmBody; +typedef struct _mjmJoint { + mjElement element; // compiler only, do not modify + mjString name; // name + mjString classname; // class name + + // joint properties + mjtJoint type; // type of Joint + int group; // used for rendering + int limited; // does joint have limits: 0 false, 1 true, 2 auto + int actfrclimited; // are actuator forces on joints limited: 0 false, 1 true, 2 auto + double pos[3]; // anchor position + double axis[3]; // joint axis + double stiffness; // stiffness coefficient + double springdamper[2]; // timeconst, dampratio + double range[2]; // joint limits + double actfrcrange[2]; // actuator force limits + mjtNum solref_limit[mjNREF]; // solver reference: joint limits + mjtNum solimp_limit[mjNIMP]; // solver impedance: joint limits + mjtNum solref_friction[mjNREF]; // solver reference: dof friction + mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction + double margin; // margin value for joint limit detection + double ref; // value at reference configuration: qpos0 + double springref; // spring reference value: qpos_spring + mjDouble userdata; // user data + + // dof properties + double armature; // armature inertia (mass for slider) + double damping; // damping coefficient + double frictionloss; // friction loss + + // other attributes + mjString info; // message appended to errors + double urdfeffort; // store effort field from urdf +} mjmJoint; + + typedef struct _mjmGeom { mjElement element; // compiler only, do not modify mjString name; // name @@ -131,6 +167,50 @@ typedef struct _mjmSite { } mjmSite; +typedef struct _mjmCamera { + mjElement element; // compiler only, do not modify + mjString name; // name + mjString classname; // class name + mjString info; // message appended to errors + mjtCamLight mode; // tracking mode + mjString targetbody; // target body for orientation + double fovy; // y-field of view + double ipd; // inter-pupilary distance + double pos[3]; // position + double quat[4]; // orientation + float intrinsic[4]; // camera intrinsics [length] + float sensor_size[2]; // sensor size [length] + float resolution[2]; // resolution [pixel] + float focal_length[2]; // focal length [length] + float focal_pixel[2]; // focal length [pixel] + float principal_length[2]; // principal point [length] + float principal_pixel[2]; // principal point [pixel] + mjDouble userdata; // user data + mjmOrientation alt; // alternative orientation specification +} mjmCamera; + + +typedef struct _mjmLight { + mjElement element; // compiler only, do not modify + mjString name; // name + mjString classname; // class name + mjString info; // message appended to errors + mjtCamLight mode; // tracking mode + mjString targetbody; // target body for orientation + mjtByte directional; // directional light + mjtByte castshadow; // does light cast shadows + mjtByte active; // is light active + double pos[3]; // position + double dir[3]; // direction + float attenuation[3]; // OpenGL attenuation (quadratic model) + float cutoff; // OpenGL cutoff + float exponent; // OpenGL exponent + float ambient[3]; // ambient color + float diffuse[3]; // diffuse color + float specular[3]; // specular color +} mjmLight; + + //---------------------------------- Public API ---------------------------------------------------- // Create model. @@ -139,6 +219,9 @@ MJAPI void* mjm_createModel(); // Delete model. MJAPI void mjm_deleteModel(void* modelspec); +// Copy spec into private attributes. +MJAPI void mjm_finalize(mjElement object); + // Add child body to body, return child spec. MJAPI mjmBody* mjm_addBody(mjmBody* body, void* defspec); @@ -146,19 +229,19 @@ MJAPI mjmBody* mjm_addBody(mjmBody* body, void* defspec); MJAPI mjmSite* mjm_addSite(mjmBody* body, void* defspec); // Add joint to body. -MJAPI void* mjm_addJoint(mjmBody* body, void* defspec); +MJAPI mjmJoint* mjm_addJoint(mjmBody* body, void* defspec); // Add freejoint to body. -MJAPI void* mjm_addFreeJoint(mjmBody* body); +MJAPI mjmJoint* mjm_addFreeJoint(mjmBody* body); // Add geom to body. MJAPI mjmGeom* mjm_addGeom(mjmBody* body, void* defspec); // Add camera to body. -MJAPI void* mjm_addCamera(mjmBody* body, void* defspec); +MJAPI mjmCamera* mjm_addCamera(mjmBody* body, void* defspec); // Add light to body. -MJAPI void* mjm_addLight(mjmBody* body, void* defspec); +MJAPI mjmLight* mjm_addLight(mjmBody* body, void* defspec); // Add frame to body. MJAPI void* mjm_addFrame(mjmBody* body, void* parentframe); @@ -208,12 +291,21 @@ MJAPI const char* mjm_setFullInertia(mjmBody* body, double quat[4], double inert // Default body attributes. MJAPI void mjm_defaultBody(mjmBody& body); +// Default joint attributes. +MJAPI void mjm_defaultJoint(mjmJoint& joint); + // Default geom attributes. MJAPI void mjm_defaultGeom(mjmGeom& geom); // Default site attributes. MJAPI void mjm_defaultSite(mjmSite& site); +// Default camera attributes. +MJAPI void mjm_defaultCamera(mjmCamera& camera); + +// Default light attributes. +MJAPI void mjm_defaultLight(mjmLight& light); + #ifdef __cplusplus } #endif diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index ef05e186..715cb7d2 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -116,7 +116,7 @@ bool mjCComposite::AddDefaultJoint(char* error, int error_sz) { return false; } else { mjCDef jnt; - jnt.joint.group = 3; + jnt.joint.spec.group = 3; defjoint[(mjtCompKind)i].push_back(jnt); } } @@ -457,8 +457,8 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int // add slider joints if none defined if (!add[mjCOMPKIND_PARTICLE]) { for (int i=0; i<3; i++) { - mjCJoint* jnt = (mjCJoint*)mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0]); - jnt->def = (mjCDef*)mjm_getDefault(body->element); + mjmJoint* jnt = mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0]); + mjm_setDefault(jnt->element, mjm_getDefault(body->element)); jnt->type = mjJNT_SLIDE; mjuu_setvec(jnt->pos, 0, 0, 0); mjuu_setvec(jnt->axis, 0, 0, 0); @@ -469,8 +469,8 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjmBody* body, char* error, int // add user-specified joints else { for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) { - mjCJoint* jnt = (mjCJoint*)mjm_addJoint(b, &defjnt); - jnt->def = (mjCDef*)mjm_getDefault(body->element); + mjmJoint* jnt = mjm_addJoint(b, &defjnt); + mjm_setDefault(jnt->element, mjm_getDefault(body->element)); } } @@ -626,12 +626,12 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjmBody* body, char* error, int err } // add slider joint - mjCJoint* jnt[3]; + mjmJoint* jnt[3]; for (int i=0; i<3; i++) { - jnt[i] = (mjCJoint*)mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0]); - jnt[i]->def = (mjCDef*)mjm_getDefault(body->element); + jnt[i] = mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0]); + mjm_setDefault(jnt[i]->element, mjm_getDefault(body->element)); mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), i, ix, iy); - jnt[i]->name = txt; + mjm_setString(jnt[i]->name, txt); jnt[i]->type = mjJNT_SLIDE; mjuu_setvec(jnt[i]->pos, 0, 0, 0); mjuu_setvec(jnt[i]->axis, 0, 0, 0); @@ -853,13 +853,13 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN // add curvature joint if (!first || strcmp(initial.c_str(), "none")) { - mjCJoint* jnt = (mjCJoint*)mjm_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0]); - jnt->def = (mjCDef*)mjm_getDefault(body->element); + mjmJoint* jnt = mjm_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0]); + mjm_setDefault(jnt->element, 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; jnt->frictionloss = jnt->type==mjJNT_FREE ? 0 : jnt->frictionloss; - jnt->name = this_joint; + mjm_setString(jnt->name, this_joint); } // exclude contact pair @@ -997,10 +997,10 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i // add main joint for (int i=0; i<2; i++) { // add joint - mjCJoint* jnt = (mjCJoint*)mjm_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0]); - jnt->def = (mjCDef*)mjm_getDefault(body->element); + mjmJoint* jnt = mjm_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0]); + mjm_setDefault(jnt->element, mjm_getDefault(body->element)); mju::sprintf_arr(txt, "%sJ%d_%d", prefix.c_str(), i, ix1); - jnt->name = txt; + mjm_setString(jnt->name, txt); jnt->type = mjJNT_HINGE; mjuu_setvec(jnt->pos, -0.5*dx, 0, 0); mjuu_setvec(jnt->axis, 0, 0, 0); @@ -1010,10 +1010,10 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i // add twist joint if (add[mjCOMPKIND_TWIST]) { // add joint - mjCJoint* jnt = (mjCJoint*)mjm_addJoint(body, &defjoint[mjCOMPKIND_TWIST][0]); - jnt->def = (mjCDef*)mjm_getDefault(body->element); + mjmJoint* jnt = mjm_addJoint(body, &defjoint[mjCOMPKIND_TWIST][0]); + mjm_setDefault(jnt->element, mjm_getDefault(body->element)); mju::sprintf_arr(txt, "%sJT%d", prefix.c_str(), ix1); - jnt->name = txt; + mjm_setString(jnt->name, txt); jnt->type = mjJNT_HINGE; mjuu_setvec(jnt->pos, -0.5*dx, 0, 0); mjuu_setvec(jnt->axis, 1, 0, 0); @@ -1022,16 +1022,16 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_TWIST); eq->def = model->defaults[0]; eq->type = mjEQ_JOINT; - eq->name1 = jnt->name; + eq->name1 = mjm_getString(jnt->name); } // add stretch joint if (add[mjCOMPKIND_STRETCH]) { // add joint - mjCJoint* jnt = (mjCJoint*)mjm_addJoint(body, &defjoint[mjCOMPKIND_STRETCH][0]); - jnt->def = (mjCDef*)mjm_getDefault(body->element); + mjmJoint* jnt = mjm_addJoint(body, &defjoint[mjCOMPKIND_STRETCH][0]); + mjm_setDefault(jnt->element, mjm_getDefault(body->element)); mju::sprintf_arr(txt, "%sJS%d", prefix.c_str(), ix1); - jnt->name = txt; + mjm_setString(jnt->name, txt); jnt->type = mjJNT_SLIDE; mjuu_setvec(jnt->pos, -0.5*dx, 0, 0); mjuu_setvec(jnt->axis, 1, 0, 0); @@ -1040,7 +1040,7 @@ mjmBody* mjCComposite::AddRopeBody(mjCModel* model, mjmBody* body, int ix, int i mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_STRETCH); eq->def = model->defaults[0]; eq->type = mjEQ_JOINT; - eq->name1 = jnt->name; + eq->name1 = mjm_getString(jnt->name); } return body; @@ -1149,10 +1149,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro } // add slider joint - mjCJoint* jnt = (mjCJoint*)mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0]); - jnt->def = (mjCDef*)mjm_getDefault(body->element); + mjmJoint* jnt = mjm_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0]); + mjm_setDefault(jnt->element, mjm_getDefault(body->element)); mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), ix, iy, iz); - jnt->name = txt; + mjm_setString(jnt->name, txt); jnt->type = mjJNT_SLIDE; mjuu_setvec(jnt->pos, 0, 0, 0); mjuu_setvec(jnt->axis, 0, 0, 1); @@ -1161,10 +1161,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjmBody* body, char* error, int erro mjCEquality* eq = model->AddEquality(def + mjCOMPKIND_JOINT); eq->def = model->defaults[0]; eq->type = mjEQ_JOINT; - eq->name1 = jnt->name; + eq->name1 = mjm_getString(jnt->name); // add joint to tendon - ten->WrapJoint(jnt->name, 1); + ten->WrapJoint(std::string(mjm_getString(jnt->name)), 1); // add neighbor constraints for (int i=0; i<3; i++) { diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 15842320..a817d7b2 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -429,7 +429,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz // add radial slider if (radial) { - mjCJoint* jnt = (mjCJoint*)mjm_addJoint(pb, 0); + mjmJoint* jnt = mjm_addJoint(pb, 0); // set properties jnt->type = mjJNT_SLIDE; @@ -442,7 +442,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz else { for (int j=0; j<3; j++) { // add joint to body - mjCJoint* jnt = (mjCJoint*)mjm_addJoint(pb, 0); + mjmJoint* jnt = mjm_addJoint(pb, 0); // set properties jnt->type = mjJNT_SLIDE; diff --git a/src/user/user_init.cc b/src/user/user_init.cc index 3481976b..1272a0f7 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -38,8 +38,23 @@ void mjm_defaultBody(mjmBody& body) { } + +// default joint attributes +void mjm_defaultJoint(mjmJoint& joint) { + memset(&joint, 0, sizeof(mjmJoint)); + joint.type = mjJNT_HINGE; + joint.axis[2] = 1; + joint.limited = 2; + joint.actfrclimited = 2; + mj_defaultSolRefImp(joint.solref_limit, joint.solimp_limit); + mj_defaultSolRefImp(joint.solref_friction, joint.solimp_friction); + joint.urdfeffort = -1; +} + + + // default geom attributes -MJAPI void mjm_defaultGeom(mjmGeom& geom) { +void mjm_defaultGeom(mjmGeom& geom) { memset(&geom, 0, sizeof(mjmGeom)); // set non-zero defaults @@ -83,3 +98,36 @@ void mjm_defaultSite(mjmSite& site) { site.rgba[0] = site.rgba[1] = site.rgba[2] = 0.5f; site.rgba[3] = 1.0f; } + + + +// default cam attributes +void mjm_defaultCamera(mjmCamera& cam) { + memset(&cam, 0, sizeof(mjmCamera)); + cam.mode = mjCAMLIGHT_FIXED; + cam.quat[0] = 1; + cam.fovy = 45; + cam.ipd = 0.068; + cam.resolution[0] = cam.resolution[1] = 1; + cam.alt.axisangle[0] = cam.alt.xyaxes[0] = cam.alt.zaxis[0] = cam.alt.euler[0] = mjNAN; +} + + + +// default light attributes +void mjm_defaultLight(mjmLight& light) { + memset(&light, 0, sizeof(mjmLight)); + light.mode = mjCAMLIGHT_FIXED; + light.directional = 0; + light.castshadow = 1; + light.active = 1; + light.dir[2] = -1; + light.attenuation[0] = 1; + light.cutoff = 45; + light.exponent = 10; + light.ambient[0] = light.ambient[1] = light.ambient[2] = 0; + light.diffuse[0] = light.diffuse[1] = light.diffuse[2] = 0.7; + light.specular[0] = light.specular[1] = light.specular[2] = 0.3; +} + + diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 10d018cf..d82c2922 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -565,7 +565,7 @@ mjCDef* mjCModel::AddDef(string name, int parentid) { if (parentid>=0 && parentidchildid.push_back(thisid); - def->MakePointerLocal(); + def->PointToLocal(); } def->parentid = parentid; def->name = name; @@ -1573,7 +1573,7 @@ void mjCModel::CopyTree(mjModel* m) { copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP); m->jnt_margin[jid] = (mjtNum)pj->margin; - copyvec(m->jnt_user+nuser_jnt*jid, pj->userdata.data(), nuser_jnt); + copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt); // not simple if: rotation already found, or pos not zero, or mis-aligned axis if (rotfound || @@ -1756,7 +1756,7 @@ void mjCModel::CopyTree(mjModel* m) { copyvec(m->cam_resolution+2*cid, pc->resolution, 2); copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2); copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4); - copyvec(m->cam_user+nuser_cam*cid, pc->userdata.data(), nuser_cam); + copyvec(m->cam_user+nuser_cam*cid, pc->get_userdata().data(), nuser_cam); } // loop over lights for this body @@ -2890,7 +2890,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { if (nuser_jnt == -1) { nuser_jnt = 0; for (int i=0; iuserdata.size()); + nuser_jnt = mjMAX(nuser_jnt, joints[i]->spec_userdata_.size()); } } if (nuser_geom == -1) { @@ -2908,7 +2908,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { if (nuser_cam == -1) { nuser_cam = 0; for (int i=0; iuserdata.size()); + nuser_cam = mjMAX(nuser_cam, cameras[i]->spec_userdata_.size()); } } if (nuser_tendon == -1) { @@ -3341,7 +3341,7 @@ bool mjCModel::CopyBack(const mjModel* m) { pj->margin = (double)m->jnt_margin[i]; if (nuser_jnt) { - copyvec(pj->userdata.data(), m->jnt_user + nuser_jnt*i, nuser_jnt); + copyvec(pj->userdata_.data(), m->jnt_user + nuser_jnt*i, nuser_jnt); } // dof data @@ -3422,7 +3422,7 @@ bool mjCModel::CopyBack(const mjModel* m) { copyvec(cameras[i]->intrinsic, m->cam_intrinsic+4*i, 4); if (nuser_cam) { - copyvec(cameras[i]->userdata.data(), m->cam_user + nuser_cam*i, nuser_cam); + copyvec(cameras[i]->userdata_.data(), m->cam_user + nuser_cam*i, nuser_cam); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 6c02e16d..052a2eb9 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -470,10 +470,10 @@ mjCDef::mjCDef(void) { // compiler void mjCDef::Compile(const mjCModel* model) { // enforce length of all default userdata arrays - joint.userdata.resize(model->nuser_jnt); + joint.userdata_.resize(model->nuser_jnt); geom.userdata_.resize(model->nuser_geom); site.userdata_.resize(model->nuser_site); - camera.userdata.resize(model->nuser_cam); + camera.userdata_.resize(model->nuser_cam); tendon.userdata.resize(model->nuser_tendon); actuator.userdata.resize(model->nuser_actuator); } @@ -481,8 +481,11 @@ void mjCDef::Compile(const mjCModel* model) { // assignment operator (TODO: use overloading) -void mjCDef::MakePointerLocal() { - geom.MakePointerLocal(); +void mjCDef::PointToLocal() { + joint.PointToLocal(); + geom.PointToLocal(); + site.PointToLocal(); + camera.PointToLocal(); } @@ -581,16 +584,21 @@ mjCBody::mjCBody(mjCModel* _model) { lights.clear(); spec_userdata_.clear(); - // point to local + // in case this body is not compiled + CopyFromSpec(); + + // point to local (needs to be after defaults) + PointToLocal(); +} + + +void mjCBody::PointToLocal() { 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(); } @@ -664,7 +672,7 @@ mjCFrame* mjCBody::AddFrame(mjCFrame* _frame) { mjCJoint* mjCBody::AddFreeJoint() { // create free joint, don't inherit from defaults mjCJoint* obj = new mjCJoint(model, NULL); - obj->type = mjJNT_FREE; + obj->spec.type = mjJNT_FREE; // set body pointer, add obj->body = this; @@ -1200,56 +1208,59 @@ void mjCFrame::Compile() { // initialize default joint mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) { - // joint defaults - type = mjJNT_HINGE; - group = 0; - mjuu_setvec(pos, 0, 0, 0); - mjuu_setvec(axis, 0, 0, 1); - limited = 2; - actfrclimited = 2; - stiffness = 0; - range[0] = 0; - range[1] = 0; - actfrcrange[0] = 0; - actfrcrange[1] = 0; - springdamper[0] = 0; - springdamper[1] = 0; - mj_defaultSolRefImp(solref_limit, solimp_limit); - mj_defaultSolRefImp(solref_friction, solimp_friction); - margin = 0; - ref = 0; - springref = 0; - userdata.clear(); - - // dof defaults - armature = 0; - frictionloss = 0; - damping = 0; + mjm_defaultJoint(spec); // clear internal variables + spec_userdata_.clear(); body = 0; - urdfeffort = -1; // reset to default if given if (_def) { + _def->joint.CopyFromSpec(); *this = _def->joint; } // set model, def model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); + + // point to local (needs to be after defaults) + PointToLocal(); + + // in case this joint is not compiled + CopyFromSpec(); +} + + + +void mjCJoint::PointToLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.userdata = (mjDouble)&spec_userdata_; + spec.info = (mjString)&info; +} + + + +void mjCJoint::CopyFromSpec() { + *static_cast(this) = spec; + userdata_ = spec_userdata_; + userdata = (mjDouble)&spec_userdata_; } // compiler int mjCJoint::Compile(void) { + CopyFromSpec(); + // resize userdata - if (userdata.size() > model->nuser_jnt) { + if (userdata_.size() > model->nuser_jnt) { throw mjCError(this, "user has more values than nuser_jnt in joint '%s' (id = %d)", name.c_str(), id); } - userdata.resize(model->nuser_jnt); + userdata_.resize(model->nuser_jnt); // check springdamper if (springdamper[0] || springdamper[1]) { @@ -1405,7 +1416,7 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) { def = (_def ? _def : (_model ? _model->defaults[0] : 0)); // point to local (needs to be after defaults) - MakePointerLocal(); + PointToLocal(); // in case this geom is not compiled CopyFromSpec(); @@ -1414,7 +1425,7 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) { // to be called after any default copy constructor -void mjCGeom::MakePointerLocal(void) { +void mjCGeom::PointToLocal(void) { spec.element = (mjElement)this; spec.name = (mjString)&name; spec.info = (mjString)&info; @@ -2001,20 +2012,26 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) { *this = _def->site; } - // point to local, not to default + // point to local (needs to be after defaults) + PointToLocal(); + + // in case this site is not compiled + CopyFromSpec(); + + // set model, def + model = _model; + def = (_def ? _def : (_model ? _model->defaults[0] : 0)); +} + + + +void mjCSite::PointToLocal() { spec.element = (mjElement)this; spec.name = (mjString)&name; spec.info = (mjString)&info; spec.classname = (mjString)&classname; spec.material = (mjString)&spec_material_; spec.userdata = (mjDouble)&spec_userdata_; - - // initialize private attributes in case object won't be compiled - CopyFromSpec(); - - // set model, def - model = _model; - def = (_def ? _def : (_model ? _model->defaults[0] : 0)); } @@ -2124,49 +2141,70 @@ void mjCSite::Compile(void) { // initialize defaults mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) { - // set defaults - mode = mjCAMLIGHT_FIXED; - targetbody.clear(); - mjuu_setvec(pos, 0, 0, 0); - mjuu_setvec(quat, 1, 0, 0, 0); - fovy = 45; - ipd = 0.068; - userdata.clear(); - resolution[0] = resolution[1] = 1; - principal_length[0] = principal_length[1] = 0; - principal_pixel[0] = principal_pixel[1] = 0; - focal_length[0] = focal_length[1] = 0; - focal_pixel[0] = focal_pixel[1] = 0; - sensor_size[0] = sensor_size[1] = 0; - mjuu_setvec(intrinsic, 0, 0, 0, 0); + mjm_defaultCamera(spec); // clear private variables body = 0; targetbodyid = -1; + spec_targetbody_.clear(); // reset to default if given if (_def) { + _def->camera.CopyFromSpec(); *this = _def->camera; } // set model, def model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); + + // point to local (needs to be after defaults) + PointToLocal(); + + // in case this camera is not compiled + CopyFromSpec(); +} + + + +void mjCCamera::PointToLocal() { + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.userdata = (mjDouble)&spec_userdata_; + spec.targetbody = (mjString)&spec_targetbody_; + spec.info = (mjString)&info; +} + + + +void mjCCamera::CopyFromSpec() { + *static_cast(this) = spec; + userdata_ = spec_userdata_; + targetbody_ = spec_targetbody_; + userdata = (mjDouble)&userdata_; + targetbody = (mjString)&targetbody_; + 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); } // compiler void mjCCamera::Compile(void) { + CopyFromSpec(); + // resize userdata - if (userdata.size() > model->nuser_cam) { + if (userdata_.size() > model->nuser_cam) { throw mjCError(this, "user has more values than nuser_cam in camera '%s' (id = %d)", name.c_str(), id); } - userdata.resize(model->nuser_cam); + userdata_.resize(model->nuser_cam); // process orientation specifications - 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, "orientation specification error '%s' in camera %d", err, id); } @@ -2180,8 +2218,8 @@ void mjCCamera::Compile(void) { mjuu_normvec(quat, 4); // get targetbodyid - if (!targetbody.empty()) { - mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody); + if (!targetbody_.empty()) { + mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody_); if (tb) { targetbodyid = tb->id; } else { @@ -2240,39 +2278,47 @@ void mjCCamera::Compile(void) { // initialize defaults mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) { - // set defaults - mode = mjCAMLIGHT_FIXED; - targetbody.clear(); - directional = false; - castshadow = true; - active = true; - mjuu_setvec(pos, 0, 0, 0); - mjuu_setvec(dir, 0, 0, -1); - mjuu_setvec(attenuation, 1, 0, 0); - cutoff = 45; - exponent = 10; - ambient[0] = ambient[1] = ambient[2] = 0; - diffuse[0] = diffuse[1] = diffuse[2] = 0.7; - specular[0] = specular[1] = specular[2] = 0.3; + mjm_defaultLight(spec); // clear private variables body = 0; targetbodyid = -1; + spec_targetbody_.clear(); // reset to default if given if (_def) { + _def->light.CopyFromSpec(); *this = _def->light; } // set model, def model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); + + // point to local + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.classname = (mjString)&classname; + spec.targetbody = (mjString)&spec_targetbody_; + spec.info = (mjString)&info; + + CopyFromSpec(); +} + + + +void mjCLight::CopyFromSpec() { + *static_cast(this) = spec; + targetbody_ = spec_targetbody_; + targetbody = (mjString)&targetbody_; } // compiler void mjCLight::Compile(void) { + CopyFromSpec(); + double quat[4]= {1, 0, 0, 0}; // frame @@ -2286,8 +2332,8 @@ void mjCLight::Compile(void) { } // get targetbodyid - if (!targetbody.empty()) { - mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody); + if (!targetbody_.empty()) { + mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody_); if (tb) { targetbodyid = tb->id; } else { @@ -4077,9 +4123,9 @@ void mjCActuator::Compile(void) { pjnt = (mjCJoint*) ptarget; // apply urdfeffort - if (pjnt->urdfeffort>0) { - forcerange[0] = -pjnt->urdfeffort; - forcerange[1] = pjnt->urdfeffort; + if (pjnt->spec.urdfeffort>0) { + forcerange[0] = -pjnt->spec.urdfeffort; + forcerange[1] = pjnt->spec.urdfeffort; forcelimited = 1; } break; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index fdf79714..6ff9a30a 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -180,6 +180,9 @@ class mjCBase { // Add frame transformation void SetFrame(mjCFrame* _frame); + // Copy spec into private attributes + virtual void CopyFromSpec() {} + std::string name; // object name std::string classname; // defaults class name int id; // object id @@ -194,6 +197,7 @@ class mjCBase { std::string plugin_instance_name; protected: mjCBase(); // constructor + virtual ~mjCBase() = default; // destructor }; @@ -289,6 +293,7 @@ class mjCBody : public mjCBase, private mjmBody { std::vector lights; // lights attached to this body void CopyFromSpec(); // copy spec into attributes + void PointToLocal(void); // variable-size data std::vector userdata_; @@ -322,48 +327,36 @@ class mjCFrame : public mjCBase { //------------------------- class mjCJoint --------------------------------------------------------- // Describes a motion degree of freedom of a body relative to its parent -class mjCJoint : public mjCBase { +class mjCJoint : public mjCBase, private mjmJoint { friend class mjCDef; friend class mjCEquality; friend class mjCBody; friend class mjCModel; + friend class mjCSensor; friend class mjXWriter; friend class mjXURDF; public: - // variables set by user: joint properties - mjtJoint type; // type of Joint - int group; // used for rendering - int limited; // does joint have limits: 0 false, 1 true, 2 auto - int actfrclimited; // are actuator forces on joints limited: 0 false, 1 true, 2 auto - double pos[3]; // anchor position - double axis[3]; // joint axis - double stiffness; // stiffness coefficient - double springdamper[2]; // timeconst, dampratio - double range[2]; // joint limits - double actfrcrange[2]; // actuator force limits - mjtNum solref_limit[mjNREF]; // solver reference: joint limits - mjtNum solimp_limit[mjNIMP]; // solver impedance: joint limits - mjtNum solref_friction[mjNREF]; // solver reference: dof friction - mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction - double margin; // margin value for joint limit detection - double ref; // value at reference configuration: qpos0 - double springref; // spring reference value: qpos_spring - std::vector userdata; // user data + mjmJoint spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; - // variables set by user: dof properties - double armature; // armature inertia (mass for slider) - double damping; // damping coefficient - double frictionloss; // friction loss + void CopyFromSpec(void); - double urdfeffort; // store effort field from urdf + // used by mjXWriter and mjCModel + const std::vector& get_userdata() { return userdata_; } private: mjCJoint(mjCModel* = 0, mjCDef* = 0); int Compile(void); // compiler; return dofnum + void PointToLocal(void); mjCBody* body; // joint's body + // variable-size data + std::vector userdata_; + std::vector spec_userdata_; }; @@ -412,7 +405,7 @@ class mjCGeom : public mjCBase, private mjmGeom { double GetRBound(void); // compute bounding sphere radius void ComputeAABB(void); // compute axis-aligned bounding box void CopyFromSpec(void); - void MakePointerLocal(void); + void PointToLocal(void); mjCAlternative alt_; bool visual_; // true: geom does not collide and is unreferenced @@ -470,6 +463,7 @@ class mjCSite : public mjCBase, private mjmSite { mjCSite(mjCModel* = 0, mjCDef* = 0); // constructor void Compile(void); // compiler void CopyFromSpec(); // copy spec into attributes + void PointToLocal(void); mjCAlternative alt_; @@ -489,36 +483,36 @@ class mjCSite : public mjCBase, private mjmSite { //------------------------- class mjCCamera -------------------------------------------------------- // Describes a camera, attached to a body -class mjCCamera : public mjCBase { +class mjCCamera : public mjCBase, private mjmCamera { friend class mjCDef; friend class mjCBody; friend class mjCModel; + friend class mjCSensor; friend class mjXWriter; public: - // variables set by user - mjtCamLight mode; // tracking mode - std::string targetbody; // target body for orientation - double fovy; // y-field of view - double ipd; // inter-pupilary distance - double pos[3]; // position - double quat[4]; // orientation - float intrinsic[4]; // camera intrinsics [length] - float sensor_size[2]; // sensor size [length] - float resolution[2]; // resolution [pixel] - float focal_length[2]; // focal length [length] - float focal_pixel[2]; // focal length [pixel] - float principal_length[2]; // principal point [length] - float principal_pixel[2]; // principal point [pixel] - std::vector userdata; // user data - mjCAlternative alt; // alternative orientation specification + mjmCamera spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; + + // used by mjXWriter and mjCModel + const std::string& get_targetbody() { return targetbody_; } + const std::vector& get_userdata() { return userdata_; } private: mjCCamera(mjCModel* = 0, mjCDef* = 0); // constructor void Compile(void); // compiler + void CopyFromSpec(void); + void PointToLocal(void); mjCBody* body; // camera's body int targetbodyid; // id of target body; -1: none + mjCAlternative alt_; + std::string targetbody_; + std::string spec_targetbody_; + std::vector userdata_; + std::vector spec_userdata_; }; @@ -526,34 +520,30 @@ class mjCCamera : public mjCBase { //------------------------- class mjCLight --------------------------------------------------------- // Describes a light, attached to a body -class mjCLight : public mjCBase { +class mjCLight : public mjCBase, private mjmLight { friend class mjCDef; friend class mjCBody; friend class mjCModel; friend class mjXWriter; public: - // variables set by user - mjtCamLight mode; // tracking mode - std::string targetbody; // target body for orientation - bool directional; // directional light - bool castshadow; // does light cast shadows - bool active; // is light active - double pos[3]; // position - double dir[3]; // direction - float attenuation[3]; // OpenGL attenuation (quadratic model) - float cutoff; // OpenGL cutoff - float exponent; // OpenGL exponent - float ambient[3]; // ambient color - float diffuse[3]; // diffuse color - float specular[3]; // specular color + mjmLight spec; + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; + + // used by mjXWriter and mjCModel + const std::string& get_targetbody() { return targetbody_; } private: mjCLight(mjCModel* = 0, mjCDef* = 0); // constructor void Compile(void); // compiler + void CopyFromSpec(void); mjCBody* body; // light's body int targetbodyid; // id of target body; -1: none + std::string targetbody_; + std::string spec_targetbody_; }; @@ -1314,7 +1304,7 @@ class mjCDef { public: mjCDef(void); // constructor void Compile(const mjCModel* model); // compiler - void MakePointerLocal(); + void PointToLocal(); // identifiers std::string name; // class name diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 8bbe389c..5fb376ca 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1465,13 +1465,18 @@ void mjXReader::OneMaterial(XMLElement* elem, mjCMaterial* pmat) { // joint element parser -void mjXReader::OneJoint(XMLElement* elem, mjCJoint* pjoint) { - string text; +void mjXReader::OneJoint(XMLElement* elem, mjmJoint* pjoint) { + string text, name, classname; + std::vector userdata; int n; // read attributes - ReadAttrTxt(elem, "name", pjoint->name); - ReadAttrTxt(elem, "class", pjoint->classname); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pjoint->name, name.c_str()); + } + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(pjoint->classname, classname.c_str()); + } if (MapValue(elem, "type", &n, joint_map, joint_sz)) { pjoint->type = (mjtJoint)n; } @@ -1496,9 +1501,13 @@ void mjXReader::OneJoint(XMLElement* elem, mjCJoint* pjoint) { ReadAttr(elem, "frictionloss", 1, &pjoint->frictionloss, text); // read userdata - ReadVector(elem, "user", pjoint->userdata, text); + if (ReadVector(elem, "user", userdata, text)) { + mjm_setDouble(pjoint->userdata, userdata.data(), userdata.size()); + } - GetXMLPos(elem, pjoint); + // write error info + mjm_setString(pjoint->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -1572,6 +1581,7 @@ void mjXReader::OneGeom(XMLElement* elem, mjmGeom* pgeom) { pgeom->typeinertia = (mjtGeomInertia)n; } + // write error info mjm_setString(pgeom->info, std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -1586,8 +1596,12 @@ void mjXReader::OneSite(XMLElement* elem, mjmSite& site) { std::string material; // read attributes - ReadAttrTxt(elem, "name", name); - ReadAttrTxt(elem, "class", classname); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(site.name, name.c_str()); + } + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(site.classname, classname.c_str()); + } if (MapValue(elem, "type", &n, geom_map, mjNGEOMTYPES)) { site.type = (mjtGeom)n; } @@ -1595,21 +1609,17 @@ void mjXReader::OneSite(XMLElement* elem, mjmSite& site) { ReadAttrInt(elem, "group", &site.group); ReadAttr(elem, "pos", 3, site.pos, text); ReadQuat(elem, "quat", site.quat, text); - ReadAttrTxt(elem, "material", material); + if (ReadAttrTxt(elem, "material", material)) { + mjm_setString(site.material, material.c_str()); + } ReadAttr(elem, "rgba", 4, site.rgba, text); ReadAttr(elem, "fromto", 6, site.fromto, text); ReadAlternative(elem, site.alt); - ReadVector(elem, "user", userdata, text); + if (ReadVector(elem, "user", userdata, text)) { + mjm_setDouble(site.userdata, userdata.data(), userdata.size()); + } - // set strings - mjm_setString(site.name, name.c_str()); - mjm_setString(site.classname, classname.c_str()); - mjm_setString(site.material, material.c_str()); - - // set pointers - mjm_setDouble(site.userdata, userdata.data(), userdata.size()); - - // set info + // write error info mjm_setString(site.info, std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -1617,14 +1627,21 @@ void mjXReader::OneSite(XMLElement* elem, mjmSite& site) { // camera element parser -void mjXReader::OneCamera(XMLElement* elem, mjCCamera* pcam) { +void mjXReader::OneCamera(XMLElement* elem, mjmCamera* pcam) { int n; - string text; + string text, name, classname, targetbody; + std::vector userdata; // read attributes - ReadAttrTxt(elem, "name", pcam->name); - ReadAttrTxt(elem, "class", pcam->classname); - ReadAttrTxt(elem, "target", pcam->targetbody); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pcam->name, name.c_str()); + } + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(pcam->classname, classname.c_str()); + } + if (ReadAttrTxt(elem, "target", targetbody)) { + mjm_setString(pcam->targetbody, targetbody.c_str()); + } if (MapValue(elem, "mode", &n, camlight_map, camlight_sz)) { pcam->mode = (mjtCamLight)n; } @@ -1654,22 +1671,31 @@ void mjXReader::OneCamera(XMLElement* elem, mjCCamera* pcam) { } // read userdata - ReadVector(elem, "user", pcam->userdata, text); + ReadVector(elem, "user", userdata, text); + mjm_setDouble(pcam->userdata, userdata.data(), userdata.size()); - GetXMLPos(elem, pcam); + // write error info + mjm_setString(pcam->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } // light element parser -void mjXReader::OneLight(XMLElement* elem, mjCLight* plight) { +void mjXReader::OneLight(XMLElement* elem, mjmLight* plight) { int n; - string text; + string text, name, classname, targetbody; // read attributes - ReadAttrTxt(elem, "name", plight->name); - ReadAttrTxt(elem, "class", plight->classname); - ReadAttrTxt(elem, "target", plight->targetbody); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(plight->name, name.c_str()); + } + if (ReadAttrTxt(elem, "class", classname)) { + mjm_setString(plight->classname, classname.c_str()); + } + if (ReadAttrTxt(elem, "target", targetbody)) { + mjm_setString(plight->targetbody, targetbody.c_str()); + } if (MapValue(elem, "mode", &n, camlight_map, camlight_sz)) { plight->mode = (mjtCamLight)n; } @@ -1691,7 +1717,9 @@ void mjXReader::OneLight(XMLElement* elem, mjCLight* plight) { ReadAttr(elem, "diffuse", 3, plight->diffuse, text); ReadAttr(elem, "specular", 3, plight->specular, text); - GetXMLPos(elem, plight); + // write error info + mjm_setString(plight->info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -2226,29 +2254,29 @@ void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) { // particle joint if (MapValue(ejnt, "type", &n, joint_map, joint_sz)) { - el->joint.type = (mjtJoint)n; + el->joint.spec.type = (mjtJoint)n; } - ReadAttr(ejnt, "axis", 3, el->joint.axis, text); + ReadAttr(ejnt, "axis", 3, el->joint.spec.axis, text); // solreffix, solimpfix ReadAttr(ejnt, "solreffix", mjNREF, el->equality.solref, text, false, false); ReadAttr(ejnt, "solimpfix", mjNIMP, el->equality.solimp, text, false, false); // joint attributes - MapValue(elem, "limited", &el->joint.limited, TFAuto_map, 3); - ReadAttrInt(ejnt, "group", &el->joint.group); - ReadAttr(ejnt, "solreflimit", mjNREF, el->joint.solref_limit, text, false, false); - ReadAttr(ejnt, "solimplimit", mjNIMP, el->joint.solimp_limit, text, false, false); + MapValue(elem, "limited", &el->joint.spec.limited, TFAuto_map, 3); + ReadAttrInt(ejnt, "group", &el->joint.spec.group); + ReadAttr(ejnt, "solreflimit", mjNREF, el->joint.spec.solref_limit, text, false, false); + ReadAttr(ejnt, "solimplimit", mjNIMP, el->joint.spec.solimp_limit, text, false, false); ReadAttr(ejnt, - "solreffriction", mjNREF, el->joint.solref_friction, text, false, false); + "solreffriction", mjNREF, el->joint.spec.solref_friction, text, false, false); ReadAttr(ejnt, - "solimpfriction", mjNIMP, el->joint.solimp_friction, text, false, false); - ReadAttr(ejnt, "stiffness", 1, &el->joint.stiffness, text); - ReadAttr(ejnt, "range", 2, el->joint.range, text); - ReadAttr(ejnt, "margin", 1, &el->joint.margin, text); - ReadAttr(ejnt, "armature", 1, &el->joint.armature, text); - ReadAttr(ejnt, "damping", 1, &el->joint.damping, text); - ReadAttr(ejnt, "frictionloss", 1, &el->joint.frictionloss, text); + "solimpfriction", mjNIMP, el->joint.spec.solimp_friction, text, false, false); + ReadAttr(ejnt, "stiffness", 1, &el->joint.spec.stiffness, text); + ReadAttr(ejnt, "range", 2, el->joint.spec.range, text); + ReadAttr(ejnt, "margin", 1, &el->joint.spec.margin, text); + ReadAttr(ejnt, "armature", 1, &el->joint.spec.armature, text); + ReadAttr(ejnt, "damping", 1, &el->joint.spec.damping, text); + ReadAttr(ejnt, "frictionloss", 1, &el->joint.spec.frictionloss, text); // advance ejnt = ejnt->NextSiblingElement("joint"); @@ -2510,7 +2538,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { else if (name=="material") OneMaterial(elem, &def->material); // read joint - else if (name=="joint") OneJoint(elem, &def->joint); + else if (name=="joint") OneJoint(elem, &def->joint.spec); // read geom else if (name=="geom") OneGeom(elem, &def->geom.spec); @@ -2519,10 +2547,10 @@ void mjXReader::Default(XMLElement* section, int parentid) { else if (name=="site") OneSite(elem, def->site.spec); // read camera - else if (name=="camera") OneCamera(elem, &def->camera); + else if (name=="camera") OneCamera(elem, &def->camera.spec); // read light - else if (name=="light") OneLight(elem, &def->light); + else if (name=="light") OneLight(elem, &def->light.spec); // read pair else if (name=="pair") OnePair(elem, &def->pair); @@ -2546,6 +2574,13 @@ void mjXReader::Default(XMLElement* section, int parentid) { OneActuator(elem, &def->actuator); } + // copy into private attributes + mjm_finalize(def->geom.spec.element); + mjm_finalize(def->joint.spec.element); + mjm_finalize(def->site.spec.element); + mjm_finalize(def->camera.spec.element); + mjm_finalize(def->light.spec.element); + // advance elem = elem->NextSiblingElement(); } @@ -3059,9 +3094,9 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjCFrame* frame) { } // create joint and parse - mjCJoint* pjoint = (mjCJoint*)mjm_addJoint(pbody, def); + mjmJoint* pjoint = mjm_addJoint(pbody, def); OneJoint(elem, pjoint); - pjoint->SetFrame(frame); + mjm_setFrame(pjoint->element, frame); } // freejoint sub-element @@ -3072,14 +3107,17 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjCFrame* frame) { } // create free joint without defaults - mjCJoint* pjoint = (mjCJoint*)mjm_addFreeJoint(pbody); - pjoint->SetFrame(frame); + mjmJoint* pjoint = mjm_addFreeJoint(pbody); + mjm_setFrame(pjoint->element, frame); // save defaults after creation, to make sure writing is ok - pjoint->def = def; + mjm_setDefault(pjoint->element, def); // read attributes - ReadAttrTxt(elem, "name", pjoint->name); + std::string name; + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pjoint->name, name.c_str()); + } ReadAttrInt(elem, "group", &pjoint->group); } @@ -3102,17 +3140,17 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjCFrame* frame) { // camera sub-element else if (name=="camera") { // create camera and parse - mjCCamera* pcam = (mjCCamera*)mjm_addCamera(pbody, def); + mjmCamera* pcam = mjm_addCamera(pbody, def); OneCamera(elem, pcam); - pcam->SetFrame(frame); + mjm_setFrame(pcam->element, frame); } // light sub-element else if (name=="light") { // create light and parse - mjCLight* plight = (mjCLight*)mjm_addLight(pbody, def); + mjmLight* plight = mjm_addLight(pbody, def); OneLight(elem, plight); - plight->SetFrame(frame); + mjm_setFrame(plight->element, frame); } // plugin sub-element @@ -3162,10 +3200,12 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjCFrame* frame) { // read attributes 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()); + if (ReadAttrTxt(elem, "name", name)) { + mjm_setString(pchild->name, name.c_str()); + } + if (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)) { diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 20a71e2d..38387d12 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -57,11 +57,11 @@ class mjXReader : public mjXBase { void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh); void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin); void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial); - void OneJoint(tinyxml2::XMLElement* elem, mjCJoint* pjoint); + void OneJoint(tinyxml2::XMLElement* elem, mjmJoint* pjoint); void OneGeom(tinyxml2::XMLElement* elem, mjmGeom* pgeom); void OneSite(tinyxml2::XMLElement* elem, mjmSite& site); - void OneCamera(tinyxml2::XMLElement* elem, mjCCamera* pcamera); - void OneLight(tinyxml2::XMLElement* elem, mjCLight* plight); + void OneCamera(tinyxml2::XMLElement* elem, mjmCamera* pcamera); + void OneLight(tinyxml2::XMLElement* elem, mjmLight* plight); void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair); void OneEquality(tinyxml2::XMLElement* elem, mjCEquality* pequality); void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index e329cfc0..4d9955a0 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -322,9 +322,9 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) { // userdata if (writingdefaults) { - WriteVector(elem, "user", pjoint->userdata); + WriteVector(elem, "user", pjoint->get_userdata()); } else { - WriteVector(elem, "user", pjoint->userdata, def->joint.userdata); + WriteVector(elem, "user", pjoint->get_userdata(), def->joint.get_userdata()); } } @@ -468,7 +468,7 @@ void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) { if (!writingdefaults) { WriteAttrTxt(elem, "name", pcam->name); WriteAttrTxt(elem, "class", pcam->classname); - WriteAttrTxt(elem, "target", pcam->targetbody); + WriteAttrTxt(elem, "target", pcam->get_targetbody()); WriteAttr(elem, "pos", 3, pcam->pos); WriteAttr(elem, "quat", 4, pcam->quat, unitq); } @@ -494,9 +494,9 @@ void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) { // userdata if (writingdefaults) { - WriteVector(elem, "user", pcam->userdata); + WriteVector(elem, "user", pcam->get_userdata()); } else { - WriteVector(elem, "user", pcam->userdata, def->camera.userdata); + WriteVector(elem, "user", pcam->get_userdata(), def->camera.get_userdata()); } } @@ -508,7 +508,7 @@ void mjXWriter::OneLight(XMLElement* elem, mjCLight* plight, mjCDef* def) { if (!writingdefaults) { WriteAttrTxt(elem, "name", plight->name); WriteAttrTxt(elem, "class", plight->classname); - WriteAttrTxt(elem, "target", plight->targetbody); + WriteAttrTxt(elem, "target", plight->get_targetbody()); WriteAttr(elem, "pos", 3, plight->pos); WriteAttr(elem, "dir", 3, plight->dir); } diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 0a8c8bbc..1f76bce4 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -210,8 +210,8 @@ void mjXURDF::Parse( // 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) { - mjCJoint* pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); - pjoint->name = urName[i] + "_free_joint"; + mjmJoint* pjoint = mjm_addJoint(pbody, 0); + mjm_setString(pjoint->name, (urName[i] + "_free_joint").c_str()); pjoint->type = mjJNT_FREE; } } @@ -371,7 +371,7 @@ void mjXURDF::Joint(XMLElement* joint_elem) { std::string jntname, name, text; XMLElement *elem; mjmBody *pbody, *parent, *world; - mjCJoint *pjoint=0, *pjoint1=0, *pjoint2=0; + mjmJoint *pjoint=0, *pjoint1=0, *pjoint2=0; int jointtype; // get type and name @@ -414,16 +414,16 @@ void mjXURDF::Joint(XMLElement* joint_elem) { switch (jointtype) { case 0: // revolute case 1: // continuous - pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); - pjoint->name = jntname; + pjoint = mjm_addJoint(pbody, 0); + mjm_setString(pjoint->name, jntname.c_str()); pjoint->type = mjJNT_HINGE; mjuu_setvec(pjoint->pos, 0, 0, 0); mjuu_copyvec(pjoint->axis, axis, 3); break; case 2: // prismatic - pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); - pjoint->name = jntname; + pjoint = mjm_addJoint(pbody, 0); + mjm_setString(pjoint->name, jntname.c_str()); pjoint->type = mjJNT_SLIDE; mjuu_setvec(pjoint->pos, 0, 0, 0); mjuu_copyvec(pjoint->axis, axis, 3); @@ -433,8 +433,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) { return; case 4: // floating - pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); - pjoint->name = jntname; + pjoint = mjm_addJoint(pbody, 0); + mjm_setString(pjoint->name, jntname.c_str()); pjoint->type = mjJNT_FREE; break; @@ -444,8 +444,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) { mjuu_quat2mat(mat, quat); // construct slider along x - pjoint = (mjCJoint*)mjm_addJoint(pbody, 0); - pjoint->name = jntname + "_TX"; + pjoint = mjm_addJoint(pbody, 0); + mjm_setString(pjoint->name, (jntname + "_TX").c_str()); pjoint->type = mjJNT_SLIDE; tmpaxis[0] = mat[0]; tmpaxis[1] = mat[3]; @@ -454,8 +454,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) { mjuu_copyvec(pjoint->axis, tmpaxis, 3); // construct slider along y - pjoint1 = (mjCJoint*)mjm_addJoint(pbody, 0); - pjoint1->name = jntname + "_TY"; + pjoint1 = mjm_addJoint(pbody, 0); + mjm_setString(pjoint1->name, (jntname + "_TY").c_str()); pjoint1->type = mjJNT_SLIDE; tmpaxis[0] = mat[1]; tmpaxis[1] = mat[4]; @@ -464,8 +464,8 @@ void mjXURDF::Joint(XMLElement* joint_elem) { mjuu_copyvec(pjoint1->axis, tmpaxis, 3); // construct hinge around z = locaxis - pjoint2 = (mjCJoint*)mjm_addJoint(pbody, 0); - pjoint2->name = jntname + "_RZ"; + pjoint2 = mjm_addJoint(pbody, 0); + mjm_setString(pjoint2->name, (jntname + "_RZ").c_str()); pjoint2->type = mjJNT_HINGE; mjuu_setvec(pjoint2->pos, 0, 0, 0); mjuu_copyvec(pjoint2->axis, axis, 3);