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

Add mjmJoint, mjmCamera, and mjmLight to C API.

Browse Source 
PiperOrigin-RevId: 605596008
Change-Id: Ie53ac4b99f5336380039ab5eb9b2ca4a82768b28
This commit is contained in:
Alessio Quaglino
2024-02-09 04:44:04 -08:00
committed by Copybara-Service
parent e77c3cb258
commit e3cbad280c
12 changed files with 506 additions and 282 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+16 -8
View File
@@ -38,6 +38,14 @@ void mjm_deleteModel(void* modelspec) {
// copy spec into private attributes
MJAPI void mjm_finalize(mjElement object) {
mjCBase* baseC = reinterpret_cast<mjCBase*>(object);
baseC->CopyFromSpec();
}
// add child body to body, return child spec
mjmBody* mjm_addBody(mjmBody* bodyspec, void* defspec) {
mjCDef* def = static_cast<mjCDef*>(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<mjCDef*>(defspec);
mjCBody* body = reinterpret_cast<mjCBody*>(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<mjCBody*>(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<mjCDef*>(defspec);
mjCBody* body = reinterpret_cast<mjCBody*>(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<mjCDef*>(defspec);
mjCBody* body = reinterpret_cast<mjCBody*>(bodyspec->element);
mjCLight* light = body->AddLight(def);
return light;
return &light->spec;
}
src/user/user_api.h
+96 -4
View File
@@ -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
src/user/user_composite.cc
+28 -28
View File
@@ -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++) {
src/user/user_flexcomp.cc
+2 -2
View File
@@ -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;
src/user/user_init.cc
+49 -1
View File
@@ -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;
}
src/user/user_model.cc
+7 -7
View File
@@ -565,7 +565,7 @@ mjCDef* mjCModel::AddDef(string name, int parentid) {
if (parentid>=0 && parentid<thisid) {
*def = *defaults[parentid];
defaults[parentid]->childid.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; i<joints.size(); i++) {
nuser_jnt = mjMAX(nuser_jnt, joints[i]->userdata.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; i<cameras.size(); i++) {
nuser_cam = mjMAX(nuser_cam, cameras[i]->userdata.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);
}
}
src/user/user_objects.cc
+132 -86
View File
@@ -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<mjmJoint*>(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<mjmCamera*>(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<mjmLight*>(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;
src/user/user_objects.h
+49 -59
View File
@@ -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<mjCLight*> lights; // lights attached to this body
void CopyFromSpec(); // copy spec into attributes
void PointToLocal(void);
// variable-size data
std::vector<double> 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<double> 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<double>& 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<double> userdata_;
std::vector<double> 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<double> 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<double>& 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<double> userdata_;
std::vector<double> 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
src/xml/xml_native_reader.cc
+103 -63
View File
@@ -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<double> 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<double> 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)) {
src/xml/xml_native_reader.h
+3 -3
View File
@@ -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);
src/xml/xml_native_writer.cc
+6 -6
View File
@@ -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);
}
src/xml/xml_urdf.cc
+15 -15
View File
@@ -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);