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

Add mjmBody and mjmPlugin and their compiler C APIs.

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