Add mjmFlex to C API.

PiperOrigin-RevId: 606264589
Change-Id: Ifd94f5241a3eaaf23653b136cece81c054f43202
This commit is contained in:
Alessio Quaglino
2024-02-12 08:46:57 -08:00
committed by Copybara-Service
parent ab209c41be
commit b224367a9b
11 changed files with 368 additions and 203 deletions
+48 -2
View File
@@ -19,6 +19,7 @@
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "user/user_model.h" #include "user/user_model.h"
#include "user/user_objects.h" #include "user/user_objects.h"
#include "xml/xml_util.h"
@@ -114,6 +115,15 @@ mjmLight* mjm_addLight(mjmBody* bodyspec, void* defspec) {
// add flex to model
mjmFlex* mjm_addFlex(void* model) {
mjCModel* modelC = static_cast<mjCModel*>(model);
mjCFlex* flex = modelC->AddFlex();
return &flex->spec;
}
// add frame to body // add frame to body
mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) { mjmFrame* mjm_addFrame(mjmBody* bodyspec, mjmFrame* parentframe) {
mjCFrame* parentframeC = 0; mjCFrame* parentframeC = 0;
@@ -292,8 +302,44 @@ void mjm_setString(mjString dest, const char* text) {
// split text and copy into string array
void mjm_setStringVec(mjStringVec dest, const char* text) {
std::vector<std::string>* v = reinterpret_cast<std::vector<std::string>*>(dest);
mjXUtil::String2Vector(text, *v);
}
// add text entry to destination string vector
void mjm_addToStringVec(mjStringVec dest, const char* text) {
std::vector<std::string>* v = reinterpret_cast<std::vector<std::string>*>(dest);
v->push_back(std::string(text));
}
// set int array
void mjm_setInt(mjIntVec dest, const int* array, int size) {
std::vector<int>* v = reinterpret_cast<std::vector<int>*>(dest);
v->assign(size, 0.0);
for (int i = 0; i < size; ++i) {
(*v)[i] = array[i];
}
}
// set float array
void mjm_setFloat(mjFloatVec dest, const float* array, int size) {
std::vector<float>* v = reinterpret_cast<std::vector<float>*>(dest);
v->assign(size, 0.0);
for (int i = 0; i < size; ++i) {
(*v)[i] = array[i];
}
}
// set double array // set double array
void mjm_setDouble(mjDouble dest, const double* array, int size) { void mjm_setDouble(mjDoubleVec dest, const double* array, int size) {
std::vector<double>* v = reinterpret_cast<std::vector<double>*>(dest); std::vector<double>* v = reinterpret_cast<std::vector<double>*>(dest);
v->assign(size, 0.0); v->assign(size, 0.0);
for (int i = 0; i < size; ++i) { for (int i = 0; i < size; ++i) {
@@ -312,7 +358,7 @@ const char* mjm_getString(const mjString source) {
// get double array // get double array
const double* mjm_getDouble(const mjDouble source, int* size) { const double* mjm_getDouble(const mjDoubleVec source, int* size) {
std::vector<double>* v = reinterpret_cast<std::vector<double>*>(source); std::vector<double>* v = reinterpret_cast<std::vector<double>*>(source);
if (size) { if (size) {
*size = v->size(); *size = v->size();
+73 -11
View File
@@ -30,7 +30,10 @@ extern "C" {
typedef struct _mjElement* mjElement; typedef struct _mjElement* mjElement;
typedef struct _mjString* mjString; typedef struct _mjString* mjString;
typedef struct _mjDouble* mjDouble; typedef struct _mjStringVec* mjStringVec;
typedef struct _mjIntVec* mjIntVec;
typedef struct _mjDoubleVec* mjDoubleVec;
typedef struct _mjFloatVec* mjFloatVec;
//---------------------------------- enum types (mjt) ---------------------------------------------- //---------------------------------- enum types (mjt) ----------------------------------------------
@@ -80,7 +83,7 @@ typedef struct _mjmBody { // body specification
// other // other
mjtByte mocap; // is this a mocap body mjtByte mocap; // is this a mocap body
double gravcomp; // gravity compensation double gravcomp; // gravity compensation
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjtByte explicitinertial; // whether to save the body with explicit inertial clause mjtByte explicitinertial; // whether to save the body with explicit inertial clause
mjmPlugin plugin; // passive force plugin mjmPlugin plugin; // passive force plugin
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
@@ -131,7 +134,7 @@ typedef struct _mjmJoint { // joint specification
// other // other
int group; // group int group; // group
double urdfeffort; // effort (urdf) double urdfeffort; // effort (urdf)
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmJoint; } mjmJoint;
@@ -179,7 +182,7 @@ typedef struct _mjmGeom { // geom specification
mjString hfieldname; // heightfield attached to geom mjString hfieldname; // heightfield attached to geom
mjString meshname; // mesh attached to geom mjString meshname; // mesh attached to geom
double fitscale; // scale mesh uniformly double fitscale; // scale mesh uniformly
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjmPlugin plugin; // sdf plugin mjmPlugin plugin; // sdf plugin
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmGeom; } mjmGeom;
@@ -204,7 +207,7 @@ typedef struct _mjmSite { // site specification
float rgba[4]; // rgba when material is omitted float rgba[4]; // rgba when material is omitted
// other // other
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmSite; } mjmSite;
@@ -233,7 +236,7 @@ typedef struct _mjmCamera { // camera specification
float principal_pixel[2]; // principal point (pixel) float principal_pixel[2]; // principal point (pixel)
// other // other
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmCamera; } mjmCamera;
@@ -265,6 +268,47 @@ typedef struct _mjmLight { // light specification
} mjmLight; } mjmLight;
typedef struct _mjmFlex {
mjElement element; // internal, do not modify
mjString name; // name
mjString classname; // class name
// contact properties
int contype; // contact type
int conaffinity; // contact affinity
int condim; // contact dimensionality
int priority; // contact priority
double friction[3]; // one-sided friction coefficients: slide, roll, spin
double solmix; // solver mixing for contact pairs
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
// other properties
int dim; // element dimensionality
double radius; // radius around primitive element
bool internal; // enable internal collisions
bool flatskin; // render flex skin with flat shading
int selfcollide; // mode for flex self colllision
int activelayers; // number of active element layers in 3D
int group; // group for visualizatioh
double edgestiffness; // edge stiffness
double edgedamping; // edge damping
float rgba[4]; // rgba when material is omitted
mjString material; // name of material used for rendering
// mesh properties
mjStringVec vertbody; // vertex body names
mjDoubleVec vert; // vertex positions
mjIntVec elem; // element vertex ids
mjFloatVec texcoord; // vertex texture coordinates
// other
mjString info; // message appended to compiler errors
} mjmFlex;
typedef struct _mjmMaterial { // material specification typedef struct _mjmMaterial { // material specification
mjElement element; // internal, do not modify mjElement element; // internal, do not modify
mjString name; // name mjString name; // name
@@ -323,7 +367,7 @@ typedef struct _mjmTendon { // tendon specification
int group; // group int group; // group
// other // other
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjString info; // message appended to errors mjString info; // message appended to errors
} mjmTendon; } mjmTendon;
@@ -373,7 +417,7 @@ typedef struct _mjmActuator { // actuator specification
// other // other
int group; // group int group; // group
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjmPlugin plugin; // actuator plugin mjmPlugin plugin; // actuator plugin
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmActuator; } mjmActuator;
@@ -401,7 +445,7 @@ typedef struct _mjmSensor { // sensor specfication
double noise; // noise stdev double noise; // noise stdev
// other // other
mjDouble userdata; // user data mjDoubleVec userdata; // user data
mjmPlugin plugin; // sensor plugin mjmPlugin plugin; // sensor plugin
mjString info; // message appended to compiler errors mjString info; // message appended to compiler errors
} mjmSensor; } mjmSensor;
@@ -442,6 +486,9 @@ MJAPI mjmLight* mjm_addLight(mjmBody* body, void* defspec);
// Add frame to body. // Add frame to body.
MJAPI mjmFrame* mjm_addFrame(mjmBody* body, mjmFrame* parentframe); MJAPI mjmFrame* mjm_addFrame(mjmBody* body, mjmFrame* parentframe);
// Add flex to model.
MJAPI mjmFlex* mjm_addFlex(void* model);
// Add material to model. // Add material to model.
MJAPI mjmMaterial* mjm_addMaterial(void* model, void* defspec); MJAPI mjmMaterial* mjm_addMaterial(void* model, void* defspec);
@@ -490,14 +537,26 @@ MJAPI int mjm_getId(mjElement element);
// Copy text to destination string. // Copy text to destination string.
MJAPI void mjm_setString(mjString dest, const char* text); MJAPI void mjm_setString(mjString dest, const char* text);
// Split text to entries and copy to destination string vector.
MJAPI void mjm_setStringVec(mjStringVec dest, const char* text);
// Add text entry to destination string vector.
MJAPI void mjm_addToStringVec(mjStringVec dest, const char* text);
// Copy int array to destination vector.
MJAPI void mjm_setInt(mjIntVec dest, const int* array, int size);
// Copy float array to destination vector.
MJAPI void mjm_setFloat(mjFloatVec dest, const float* array, int size);
// Copy double array to destination vector. // Copy double array to destination vector.
MJAPI void mjm_setDouble(mjDouble dest, const double* array, int size); MJAPI void mjm_setDouble(mjDoubleVec dest, const double* array, int size);
// Get string contents. // Get string contents.
MJAPI const char* mjm_getString(mjString source); MJAPI const char* mjm_getString(mjString source);
// Get double array contents and optionally its size. // Get double array contents and optionally its size.
MJAPI const double* mjm_getDouble(mjDouble source, int* size); MJAPI const double* mjm_getDouble(mjDoubleVec source, int* size);
// Set default. // Set default.
MJAPI void mjm_setDefault(mjElement element, void* defspec); MJAPI void mjm_setDefault(mjElement element, void* defspec);
@@ -532,6 +591,9 @@ MJAPI void mjm_defaultCamera(mjmCamera& camera);
// Default light attributes. // Default light attributes.
MJAPI void mjm_defaultLight(mjmLight& light); MJAPI void mjm_defaultLight(mjmLight& light);
// Default flex attributes.
MJAPI void mjm_defaultFlex(mjmFlex& flex);
// Default material attributes. // Default material attributes.
MJAPI void mjm_defaultMaterial(mjmMaterial& material); MJAPI void mjm_defaultMaterial(mjmMaterial& material);
+34 -27
View File
@@ -78,6 +78,8 @@ mjCFlexcomp::mjCFlexcomp(void) {
// make flexcomp object // make flexcomp object
bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz) { bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz) {
def.flex.CopyFromSpec();
bool radial = (type==mjFCOMPTYPE_BOX || bool radial = (type==mjFCOMPTYPE_BOX ||
type==mjFCOMPTYPE_CYLINDER || type==mjFCOMPTYPE_CYLINDER ||
type==mjFCOMPTYPE_ELLIPSOID); type==mjFCOMPTYPE_ELLIPSOID);
@@ -98,7 +100,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
} }
// check spacing // check spacing
double minspace = 2*def.flex.radius + def.flex.margin; double minspace = 2*def.flex.spec.radius + def.flex.spec.margin;
if (!direct) { if (!direct) {
if (spacing[0]<minspace || if (spacing[0]<minspace ||
spacing[1]<minspace || spacing[1]<minspace ||
@@ -156,7 +158,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
} }
// get dim and check // get dim and check
int dim = def.flex.dim; int dim = def.flex.spec.dim;
if (dim<1 || dim>3) { if (dim<1 || dim>3) {
return comperr(error, "Invalid dim, must be between 1 and 3", error_sz); return comperr(error, "Invalid dim, must be between 1 and 3", error_sz);
} }
@@ -164,7 +166,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
// force flatskin shading for box, cylinder and 3D grid // force flatskin shading for box, cylinder and 3D grid
if (type==mjFCOMPTYPE_BOX || type==mjFCOMPTYPE_CYLINDER || if (type==mjFCOMPTYPE_BOX || type==mjFCOMPTYPE_CYLINDER ||
(type==mjFCOMPTYPE_GRID && dim==3)) { (type==mjFCOMPTYPE_GRID && dim==3)) {
def.flex.flatskin = true; def.flex.spec.flatskin = true;
} }
// check pin sizes // check pin sizes
@@ -368,21 +370,22 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
} }
// create flex, copy parameters // create flex, copy parameters
mjCFlex* pf = model->AddFlex(); mjCFlex* flex = model->AddFlex();
int id = pf->id; mjmFlex* pf = &flex->spec;
*pf = def.flex; int id = flex->id;
pf->model = model;
pf->id = id; *flex = def.flex;
pf->name = name; flex->PointToLocal();
pf->elem = element;
if (!centered) { flex->model = model;
pf->vert = point; flex->id = id;
} mjm_setString(pf->name, name.c_str());
pf->texcoord = texcoord; mjm_setInt(pf->elem, element.data(), element.size());
mjm_setFloat(pf->texcoord, texcoord.data(), texcoord.size());
// rigid: set parent name, nothing else to do // rigid: set parent name, nothing else to do
if (rigid) { if (rigid) {
pf->vertbody.push_back(std::string(mjm_getString(body->name)).c_str()); mjm_addToStringVec(pf->vertbody, mjm_getString(body->name));
return true; return true;
} }
@@ -399,7 +402,7 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
// pinned: parent body // pinned: parent body
if (pinned[i]) { if (pinned[i]) {
pf->vertbody.push_back(std::string(mjm_getString(body->name)).c_str()); mjm_addToStringVec(pf->vertbody, mjm_getString(body->name));
// add plugin // add plugin
if (plugin_instance) { if (plugin_instance) {
@@ -456,13 +459,13 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
char txt[100]; char txt[100];
mju::sprintf_arr(txt, "%s_%d", name.c_str(), i); mju::sprintf_arr(txt, "%s_%d", name.c_str(), i);
mjm_setString(pb->name, txt); mjm_setString(pb->name, txt);
pf->vertbody.push_back(std::string(mjm_getString(pb->name)).c_str()); mjm_addToStringVec(pf->vertbody, mjm_getString(pb->name));
// clear flex vertex coordinates if allocated // clear flex vertex coordinates if allocated
if (!centered) { if (!centered) {
pf->vert[3*i] = 0; point[3*i] = 0;
pf->vert[3*i+1] = 0; point[3*i+1] = 0;
pf->vert[3*i+2] = 0; point[3*i+2] = 0;
} }
// add plugin // add plugin
@@ -476,6 +479,10 @@ bool mjCFlexcomp::Make(mjCModel* model, mjmBody* body, char* error, int error_sz
} }
} }
if (!centered) {
mjm_setDouble(pf->vert, point.data(), point.size());
}
// create edge equality constraint // create edge equality constraint
if (equality) { if (equality) {
mjmEquality* pe = mjm_addEquality(model, &def); mjmEquality* pe = mjm_addEquality(model, &def);
@@ -502,7 +509,7 @@ int mjCFlexcomp::GridID(int ix, int iy, int iz) {
// make grid // make grid
bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
int dim = def.flex.dim; int dim = def.flex.spec.dim;
bool hastex = texcoord.empty(); bool hastex = texcoord.empty();
// 1D // 1D
@@ -683,7 +690,7 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
double pos[3]; double pos[3];
// set 3D // set 3D
def.flex.dim = 3; def.flex.spec.dim = 3;
// add center point // add center point
point.push_back(0); point.push_back(0);
@@ -815,7 +822,7 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
} }
// check dim // check dim
if (def.flex.dim!=2) { if (def.flex.spec.dim!=2) {
return comperr(error, "Flex dim must be 2 in for mesh", error_sz); return comperr(error, "Flex dim must be 2 in for mesh", error_sz);
} }
@@ -1027,7 +1034,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
if (entityDim<1 || entityDim>3) { if (entityDim<1 || entityDim>3) {
throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
} }
def.flex.dim = entityDim; def.flex.spec.dim = entityDim;
// read and discard node tags; require range from minNodeTag to maxNodeTag // read and discard node tags; require range from minNodeTag to maxNodeTag
for (size_t i=0; i<numNodes; i++) { for (size_t i=0; i<numNodes; i++) {
@@ -1079,7 +1086,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
if (entityDim<1 || entityDim>3) { if (entityDim<1 || entityDim>3) {
throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
} }
def.flex.dim = entityDim; def.flex.spec.dim = entityDim;
// check section byte size // check section byte size
if (nodeend-nodebegin < 52+numNodes*4*8) { if (nodeend-nodebegin < 52+numNodes*4*8) {
@@ -1127,7 +1134,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
} }
// dimensionality must be same as nodes // dimensionality must be same as nodes
if (entityDim!=def.flex.dim) { if (entityDim!=def.flex.spec.dim) {
throw mjCError(NULL, "Inconsistent dimensionality in Elements"); throw mjCError(NULL, "Inconsistent dimensionality in Elements");
} }
@@ -1176,7 +1183,7 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
} }
// dimensionality must be same as nodes // dimensionality must be same as nodes
if (entityDim!=def.flex.dim) { if (entityDim!=def.flex.spec.dim) {
throw mjCError(NULL, "Inconsistent dimensionality in Elements"); throw mjCError(NULL, "Inconsistent dimensionality in Elements");
} }
+25 -1
View File
@@ -167,7 +167,31 @@ void mjm_defaultLight(mjmLight& light) {
// Default material attributes. // default flex attributes
void mjm_defaultFlex(mjmFlex& flex) {
memset(&flex, 0, sizeof(mjmFlex));
// set contact defaults
flex.contype = 1;
flex.conaffinity = 1;
flex.condim = 3;
mjuu_setvec(flex.friction, 1, 0.005, 0.0001);
flex.solmix = 1.0;
mj_defaultSolRefImp(flex.solref, flex.solimp);
// set other defaults
flex.dim = 2;
flex.radius = 0.005;
flex.internal = true;
flex.selfcollide = mjFLEXSELF_AUTO;
flex.activelayers = 1;
flex.rgba[0] = flex.rgba[1] = flex.rgba[2] = 0.5f;
flex.rgba[3] = 1.0f;
}
// default material attributes
void mjm_defaultMaterial(mjmMaterial& material) { void mjm_defaultMaterial(mjmMaterial& material) {
memset(&material, 0, sizeof(mjmMaterial)); memset(&material, 0, sizeof(mjmMaterial));
material.texuniform = false; material.texuniform = false;
+77 -66
View File
@@ -2222,34 +2222,11 @@ constexpr int eledge[3][6][2] = {{{ 0, 1}, {-1, -1}, {-1, -1},
// constructor // constructor
mjCFlex::mjCFlex(mjCModel* _model) { mjCFlex::mjCFlex(mjCModel* _model) {
mjm_defaultFlex(spec);
// set model // set model
model = _model; model = _model;
// set contact defaults
contype = 1;
conaffinity = 1;
condim = 3;
priority = 0;
mjuu_setvec(friction, 1, 0.005, 0.0001);
solmix = 1.0;
mj_defaultSolRefImp(solref, solimp);
margin = 0;
gap = 0;
// set other defaults
dim = 2;
radius = 0.005;
internal = true;
flatskin = false;
selfcollide = mjFLEXSELF_AUTO;
activelayers = 1;
group = 0;
edgestiffness = 0;
edgedamping = 0;
material_.clear();
rgba[0] = rgba[1] = rgba[2] = 0.5f;
rgba[3] = 1.0f;
// clear internal variables // clear internal variables
nvert = 0; nvert = 0;
nedge = 0; nedge = 0;
@@ -2257,50 +2234,84 @@ mjCFlex::mjCFlex(mjCModel* _model) {
matid = -1; matid = -1;
rigid = false; rigid = false;
centered = false; centered = false;
PointToLocal();
CopyFromSpec();
}
void mjCFlex::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.material = (mjString)&spec_material_;
spec.vertbody = (mjStringVec)&spec_vertbody_;
spec.vert = (mjDoubleVec)&spec_vert_;
spec.texcoord = (mjFloatVec)&spec_texcoord_;
spec.elem = (mjIntVec)&spec_elem_;
spec.info = (mjString)&info;
}
void mjCFlex::CopyFromSpec() {
*static_cast<mjmFlex*>(this) = spec;
spec.info = (mjString)&info;
material_ = spec_material_;
vertbody_ = spec_vertbody_;
vert_ = spec_vert_;
texcoord_ = spec_texcoord_;
elem_ = spec_elem_;
material = (mjString)&material_;
vertbody = (mjStringVec)&vertbody_;
vert = (mjDoubleVec)&vert_;
texcoord = (mjFloatVec)&texcoord_;
elem = (mjIntVec)&elem_;
} }
bool mjCFlex::HasTexcoord() const { bool mjCFlex::HasTexcoord() const {
return !texcoord.empty(); return !texcoord_.empty();
} }
void mjCFlex::DelTexcoord() { void mjCFlex::DelTexcoord() {
texcoord.clear(); texcoord_.clear();
} }
// compiler // compiler
void mjCFlex::Compile(const mjVFS* vfs) { void mjCFlex::Compile(const mjVFS* vfs) {
CopyFromSpec();
// set nelem; check sizes // set nelem; check sizes
if (dim<1 || dim>3) { if (dim<1 || dim>3) {
throw mjCError(this, "dim must be 1, 2 or 3"); throw mjCError(this, "dim must be 1, 2 or 3");
} }
if (elem.empty()) { if (elem_.empty()) {
throw mjCError(this, "elem is empty"); throw mjCError(this, "elem is empty");
} }
if (elem.size() % (dim+1)) { if (elem_.size() % (dim+1)) {
throw mjCError(this, "elem size must be multiple of (dim+1)"); throw mjCError(this, "elem size must be multiple of (dim+1)");
} }
if (vertbody.empty()) { if (vertbody_.empty()) {
throw mjCError(this, "vertbody is empty"); throw mjCError(this, "vertbody is empty");
} }
if (vert.size() % 3) { if (vert_.size() % 3) {
throw mjCError(this, "vert size must be a multiple of 3"); throw mjCError(this, "vert size must be a multiple of 3");
} }
if (edgestiffness>0 && dim>1) { if (edgestiffness>0 && dim>1) {
throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins"); throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins");
} }
nelem = (int)elem.size()/(dim+1); nelem = (int)elem_.size()/(dim+1);
// set nvert, rigid, centered; check size // set nvert, rigid, centered; check size
if (vert.empty()) { if (vert_.empty()) {
centered = true; centered = true;
nvert = (int)vertbody.size(); nvert = (int)vertbody_.size();
} }
else { else {
nvert = (int)vert.size()/3; nvert = (int)vert_.size()/3;
if (vertbody.size()==1) { if (vertbody_.size()==1) {
rigid = true; rigid = true;
} }
} }
@@ -2309,14 +2320,14 @@ void mjCFlex::Compile(const mjVFS* vfs) {
} }
// check elem vertex ids // check elem vertex ids
for (int i=0; i<(int)elem.size(); i++) { for (int i=0; i<(int)elem_.size(); i++) {
if (elem[i]<0 || elem[i]>=nvert) { if (elem_[i]<0 || elem_[i]>=nvert) {
throw mjCError(this, "elem vertex id out of range"); throw mjCError(this, "elem vertex id out of range");
} }
} }
// check texcoord // check texcoord
if (!texcoord.empty() && texcoord.size()!=2*nvert) { if (!texcoord_.empty() && texcoord_.size()!=2*nvert) {
throw mjCError(this, "two texture coordinates per vertex expected"); throw mjCError(this, "two texture coordinates per vertex expected");
} }
@@ -2329,20 +2340,20 @@ void mjCFlex::Compile(const mjVFS* vfs) {
} }
// resolve body ids // resolve body ids
for (int i=0; i<(int)vertbody.size(); i++) { for (int i=0; i<(int)vertbody_.size(); i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody[i]); mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody_[i]);
if (pbody) { if (pbody) {
vertbodyid.push_back(pbody->id); vertbodyid.push_back(pbody->id);
} else { } else {
throw mjCError(this, "unkown body '%s' in flex", vertbody[i].c_str()); throw mjCError(this, "unkown body '%s' in flex", vertbody_[i].c_str());
} }
} }
// process elements // process elements
for (int e=0; e<(int)elem.size()/(dim+1); e++) { for (int e=0; e<(int)elem_.size()/(dim+1); e++) {
// make sorted copy of element // make sorted copy of element
vector<int> el; vector<int> el;
el.assign(elem.begin()+e*(dim+1), elem.begin()+(e+1)*(dim+1)); el.assign(elem_.begin()+e*(dim+1), elem_.begin()+(e+1)*(dim+1));
std::sort(el.begin(), el.end()); std::sort(el.begin(), el.end());
// check for repeated vertices // check for repeated vertices
@@ -2367,8 +2378,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// determine centered if not already set // determine centered if not already set
if (!centered) { if (!centered) {
centered = true; centered = true;
for (int i=0; i<(int)vert.size(); i++) { for (int i=0; i<(int)vert_.size(); i++) {
if (vert[i]!=0) { if (vert_[i]!=0) {
centered = false; centered = false;
break; break;
} }
@@ -2385,7 +2396,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// add vertex offset within body if not centered // add vertex offset within body if not centered
if (!centered) { if (!centered) {
mjtNum offset[3]; mjtNum offset[3];
mju_rotVecQuat(offset, vert.data()+3*i, model->bodies[b]->xquat0); mju_rotVecQuat(offset, vert_.data()+3*i, model->bodies[b]->xquat0);
mju_addTo3(vertxpos.data()+3*i, offset); mju_addTo3(vertxpos.data()+3*i, offset);
} }
} }
@@ -2394,7 +2405,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2) // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2)
if (dim==3) { if (dim==3) {
for (int e=0; e<nelem; e++) { for (int e=0; e<nelem; e++) {
const int* edata = elem.data() + e*(dim+1); const int* edata = elem_.data() + e*(dim+1);
mjtNum* v0 = vertxpos.data() + 3*edata[0]; mjtNum* v0 = vertxpos.data() + 3*edata[0];
mjtNum* v1 = vertxpos.data() + 3*edata[1]; mjtNum* v1 = vertxpos.data() + 3*edata[1];
mjtNum* v2 = vertxpos.data() + 3*edata[2]; mjtNum* v2 = vertxpos.data() + 3*edata[2];
@@ -2408,22 +2419,22 @@ void mjCFlex::Compile(const mjVFS* vfs) {
mju_cross(nrm, v01, v02); mju_cross(nrm, v01, v02);
if (mju_dot3(nrm, v03)>0) { if (mju_dot3(nrm, v03)>0) {
// flip orientation // flip orientation
int tmp = elem[e*(dim+1)+1]; int tmp = elem_[e*(dim+1)+1];
elem[e*(dim+1)+1] = elem[e*(dim+1)+2]; elem_[e*(dim+1)+1] = elem_[e*(dim+1)+2];
elem[e*(dim+1)+2] = tmp; elem_[e*(dim+1)+2] = tmp;
} }
} }
} }
// create edges // create edges
std::vector<int> edgeidx(elem.size()*kNumEdges[dim-1]); std::vector<int> edgeidx(elem_.size()*kNumEdges[dim-1]);
// map from edge vertices to their index in `edges` vector // map from edge vertices to their index in `edges` vector
std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices; std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices;
// insert local edges into global vector // insert local edges into global vector
for (int f = 0; f < (int)elem.size()/(dim+1); f++) { for (int f = 0; f < (int)elem_.size()/(dim+1); f++) {
int* v = elem.data() + f*(dim+1); int* v = elem_.data() + f*(dim+1);
for (int e = 0; e < kNumEdges[dim-1]; e++) { for (int e = 0; e < kNumEdges[dim-1]; e++) {
auto pair = std::pair( auto pair = std::pair(
std::min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]), std::min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]),
@@ -2447,7 +2458,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// add plugins // add plugins
std::string userface, useredge; std::string userface, useredge;
mjXUtil::Vector2String(userface, elem); mjXUtil::Vector2String(userface, elem_);
mjXUtil::Vector2String(useredge, edgeidx); mjXUtil::Vector2String(useredge, edgeidx);
for (int i=0; i<(int)vertbodyid.size(); i++) { for (int i=0; i<(int)vertbodyid.size(); i++) {
@@ -2472,12 +2483,12 @@ void mjCFlex::CreateBVH(void) {
int nbvh = 0; int nbvh = 0;
// allocate element bounding boxes // allocate element bounding boxes
elemaabb.resize(6*nelem); elemaabb_.resize(6*nelem);
tree.AllocateBoundingVolumes(nelem); tree.AllocateBoundingVolumes(nelem);
// construct element bounding boxes, add to hierarchy // construct element bounding boxes, add to hierarchy
for (int e=0; e<nelem; e++) { for (int e=0; e<nelem; e++) {
const int* edata = elem.data() + e*(dim+1); const int* edata = elem_.data() + e*(dim+1);
// skip inactive in 3D // skip inactive in 3D
if (dim==3 && elemlayer[e]>=activelayers) { if (dim==3 && elemlayer[e]>=activelayers) {
@@ -2496,12 +2507,12 @@ void mjCFlex::CreateBVH(void) {
} }
// compute aabb (center, size) // compute aabb (center, size)
elemaabb[6*e+0] = 0.5*(xmax[0]+xmin[0]); elemaabb_[6*e+0] = 0.5*(xmax[0]+xmin[0]);
elemaabb[6*e+1] = 0.5*(xmax[1]+xmin[1]); elemaabb_[6*e+1] = 0.5*(xmax[1]+xmin[1]);
elemaabb[6*e+2] = 0.5*(xmax[2]+xmin[2]); elemaabb_[6*e+2] = 0.5*(xmax[2]+xmin[2]);
elemaabb[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius; elemaabb_[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius;
elemaabb[6*e+4] = 0.5*(xmax[1]-xmin[1]) + radius; elemaabb_[6*e+4] = 0.5*(xmax[1]-xmin[1]) + radius;
elemaabb[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius; elemaabb_[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius;
// add bounding volume for this element // add bounding volume for this element
mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++); mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++);
@@ -2509,7 +2520,7 @@ void mjCFlex::CreateBVH(void) {
bv->conaffinity = conaffinity; bv->conaffinity = conaffinity;
bv->quat = NULL; bv->quat = NULL;
bv->SetId(e); bv->SetId(e);
bv->aabb = elemaabb.data() + 6*e; bv->aabb = elemaabb_.data() + 6*e;
bv->pos = bv->aabb; bv->pos = bv->aabb;
} }
@@ -2533,7 +2544,7 @@ void mjCFlex::CreateShellPair(void) {
// element vertices in original (unsorted) order // element vertices in original (unsorted) order
vector<int> el; vector<int> el;
el.assign(elem.begin()+n, elem.begin()+n+dim+1); el.assign(elem_.begin()+n, elem_.begin()+n+dim+1);
// line: 2 vertex fragments // line: 2 vertex fragments
if (dim==1) { if (dim==1) {
@@ -2702,7 +2713,7 @@ void mjCFlex::CreateShellPair(void) {
// process both elements // process both elements
for (int ei=0; ei<2; ei++) { for (int ei=0; ei<2; ei++) {
const int* edata = elem.data() + connectspec[n][ei]*(dim+1); const int* edata = elem_.data() + connectspec[n][ei]*(dim+1);
// find element vertex that is not in the common fragment // find element vertex that is not in the common fragment
for (int i=0; i<=dim; i++) { for (int i=0; i<=dim; i++) {
+5 -5
View File
@@ -1995,15 +1995,15 @@ void mjCModel::CopyObjects(mjModel* m) {
m->flex_evpairnum[i] = (int)pfl->evpair.size()/2; m->flex_evpairnum[i] = (int)pfl->evpair.size()/2;
memcpy(m->flex_evpair + 2*evpair_adr, pfl->evpair.data(), pfl->evpair.size()*sizeof(int)); memcpy(m->flex_evpair + 2*evpair_adr, pfl->evpair.data(), pfl->evpair.size()*sizeof(int));
} }
if (pfl->texcoord.empty()) { if (pfl->texcoord_.empty()) {
m->flex_texcoordadr[i] = -1; m->flex_texcoordadr[i] = -1;
} else { } else {
m->flex_texcoordadr[i] = texcoord_adr; m->flex_texcoordadr[i] = texcoord_adr;
memcpy(m->flex_texcoord + 2*texcoord_adr, memcpy(m->flex_texcoord + 2*texcoord_adr,
pfl->texcoord.data(), pfl->texcoord.size()*sizeof(float)); pfl->texcoord_.data(), pfl->texcoord_.size()*sizeof(float));
} }
m->flex_elemnum[i] = pfl->nelem; m->flex_elemnum[i] = pfl->nelem;
memcpy(m->flex_elem + elemdata_adr, pfl->elem.data(), pfl->elem.size()*sizeof(int)); memcpy(m->flex_elem + elemdata_adr, pfl->elem_.data(), pfl->elem_.size()*sizeof(int));
memcpy(m->flex_elemlayer + elem_adr, pfl->elemlayer.data(), pfl->nelem*sizeof(int)); memcpy(m->flex_elemlayer + elem_adr, pfl->elemlayer.data(), pfl->nelem*sizeof(int));
if (m->flex_shellnum[i]) { if (m->flex_shellnum[i]) {
memcpy(m->flex_shell + shelldata_adr, pfl->shell.data(), pfl->shell.size()*sizeof(int)); memcpy(m->flex_shell + shelldata_adr, pfl->shell.data(), pfl->shell.size()*sizeof(int));
@@ -2042,7 +2042,7 @@ void mjCModel::CopyObjects(mjModel* m) {
mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert); mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
} }
else { else {
memcpy(m->flex_vert + 3*vert_adr, pfl->vert.data(), 3*pfl->nvert*sizeof(mjtNum)); memcpy(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert*sizeof(mjtNum));
} }
// copy or set vertbodyid // copy or set vertbodyid
@@ -2073,7 +2073,7 @@ void mjCModel::CopyObjects(mjModel* m) {
elemdata_adr += (pfl->dim+1) * pfl->nelem; elemdata_adr += (pfl->dim+1) * pfl->nelem;
shelldata_adr += (int)pfl->shell.size(); shelldata_adr += (int)pfl->shell.size();
evpair_adr += (int)pfl->evpair.size()/2; evpair_adr += (int)pfl->evpair.size()/2;
texcoord_adr += (int)pfl->texcoord.size()/2; texcoord_adr += (int)pfl->texcoord_.size()/2;
bvh_adr += pfl->tree.nbvh; bvh_adr += pfl->tree.nbvh;
} }
+16 -16
View File
@@ -596,7 +596,7 @@ void mjCBody::PointToLocal() {
spec.element = (mjElement)this; spec.element = (mjElement)this;
spec.name = (mjString)&name; spec.name = (mjString)&name;
spec.classname = (mjString)&classname; spec.classname = (mjString)&classname;
spec.userdata = (mjDouble)&spec_userdata_; spec.userdata = (mjDoubleVec)&spec_userdata_;
spec.plugin.name = (mjString)&plugin_name; spec.plugin.name = (mjString)&plugin_name;
spec.plugin.instance_name = (mjString)&plugin_instance_name; spec.plugin.instance_name = (mjString)&plugin_instance_name;
} }
@@ -605,7 +605,7 @@ void mjCBody::PointToLocal() {
void mjCBody::CopyFromSpec() { void mjCBody::CopyFromSpec() {
*static_cast<mjmBody*>(this) = spec; *static_cast<mjmBody*>(this) = spec;
userdata_ = spec_userdata_; userdata_ = spec_userdata_;
userdata = (mjDouble)&userdata_; userdata = (mjDoubleVec)&userdata_;
spec.info = (mjString)&info; spec.info = (mjString)&info;
mju_copy4(alt_.axisangle, alt.axisangle); mju_copy4(alt_.axisangle, alt.axisangle);
mju_copy(alt_.xyaxes, alt.xyaxes, 6); mju_copy(alt_.xyaxes, alt.xyaxes, 6);
@@ -1262,7 +1262,7 @@ void mjCJoint::PointToLocal() {
spec.element = (mjElement)this; spec.element = (mjElement)this;
spec.name = (mjString)&name; spec.name = (mjString)&name;
spec.classname = (mjString)&classname; spec.classname = (mjString)&classname;
spec.userdata = (mjDouble)&spec_userdata_; spec.userdata = (mjDoubleVec)&spec_userdata_;
spec.info = (mjString)&info; spec.info = (mjString)&info;
} }
@@ -1271,7 +1271,7 @@ void mjCJoint::PointToLocal() {
void mjCJoint::CopyFromSpec() { void mjCJoint::CopyFromSpec() {
*static_cast<mjmJoint*>(this) = spec; *static_cast<mjmJoint*>(this) = spec;
userdata_ = spec_userdata_; userdata_ = spec_userdata_;
userdata = (mjDouble)&spec_userdata_; userdata = (mjDoubleVec)&spec_userdata_;
} }
@@ -1460,7 +1460,7 @@ void mjCGeom::PointToLocal(void) {
spec.name = (mjString)&name; spec.name = (mjString)&name;
spec.info = (mjString)&info; spec.info = (mjString)&info;
spec.classname = (mjString)&classname; spec.classname = (mjString)&classname;
spec.userdata = (mjDouble)&spec_userdata_; spec.userdata = (mjDoubleVec)&spec_userdata_;
spec.material = (mjString)&spec_material_; spec.material = (mjString)&spec_material_;
spec.meshname = (mjString)&spec_meshname_; spec.meshname = (mjString)&spec_meshname_;
spec.hfieldname = (mjString)&spec_hfieldname_; spec.hfieldname = (mjString)&spec_hfieldname_;
@@ -1476,7 +1476,7 @@ void mjCGeom::CopyFromSpec() {
hfieldname_ = spec_hfieldname_; hfieldname_ = spec_hfieldname_;
meshname_ = spec_meshname_; meshname_ = spec_meshname_;
material_ = spec_material_; material_ = spec_material_;
userdata = (mjDouble)&userdata_; userdata = (mjDoubleVec)&userdata_;
hfieldname = (mjString)&hfieldname_; hfieldname = (mjString)&hfieldname_;
meshname = (mjString)&meshname_; meshname = (mjString)&meshname_;
material = (mjString)&material_; material = (mjString)&material_;
@@ -2061,7 +2061,7 @@ void mjCSite::PointToLocal() {
spec.info = (mjString)&info; spec.info = (mjString)&info;
spec.classname = (mjString)&classname; spec.classname = (mjString)&classname;
spec.material = (mjString)&spec_material_; spec.material = (mjString)&spec_material_;
spec.userdata = (mjDouble)&spec_userdata_; spec.userdata = (mjDoubleVec)&spec_userdata_;
} }
@@ -2070,7 +2070,7 @@ void mjCSite::CopyFromSpec() {
*static_cast<mjmSite*>(this) = spec; *static_cast<mjmSite*>(this) = spec;
userdata_ = spec_userdata_; userdata_ = spec_userdata_;
material_ = spec_material_; material_ = spec_material_;
userdata = (mjDouble)&userdata_; userdata = (mjDoubleVec)&userdata_;
material = (mjString)&material_; material = (mjString)&material_;
mju_copy4(alt_.axisangle, alt.axisangle); mju_copy4(alt_.axisangle, alt.axisangle);
mju_copy(alt_.xyaxes, alt.xyaxes, 6); mju_copy(alt_.xyaxes, alt.xyaxes, 6);
@@ -2201,7 +2201,7 @@ void mjCCamera::PointToLocal() {
spec.element = (mjElement)this; spec.element = (mjElement)this;
spec.name = (mjString)&name; spec.name = (mjString)&name;
spec.classname = (mjString)&classname; spec.classname = (mjString)&classname;
spec.userdata = (mjDouble)&spec_userdata_; spec.userdata = (mjDoubleVec)&spec_userdata_;
spec.targetbody = (mjString)&spec_targetbody_; spec.targetbody = (mjString)&spec_targetbody_;
spec.info = (mjString)&info; spec.info = (mjString)&info;
} }
@@ -2212,7 +2212,7 @@ void mjCCamera::CopyFromSpec() {
*static_cast<mjmCamera*>(this) = spec; *static_cast<mjmCamera*>(this) = spec;
userdata_ = spec_userdata_; userdata_ = spec_userdata_;
targetbody_ = spec_targetbody_; targetbody_ = spec_targetbody_;
userdata = (mjDouble)&userdata_; userdata = (mjDoubleVec)&userdata_;
targetbody = (mjString)&targetbody_; targetbody = (mjString)&targetbody_;
mju_copy4(alt_.axisangle, alt.axisangle); mju_copy4(alt_.axisangle, alt.axisangle);
mju_copy(alt_.xyaxes, alt.xyaxes, 6); mju_copy(alt_.xyaxes, alt.xyaxes, 6);
@@ -3717,7 +3717,7 @@ void mjCTendon::PointToLocal() {
spec.name = (mjString)&name; spec.name = (mjString)&name;
spec.classname = (mjString)&classname; spec.classname = (mjString)&classname;
spec.material = (mjString)&spec_material_; spec.material = (mjString)&spec_material_;
spec.userdata = (mjDouble)&spec_userdata_; spec.userdata = (mjDoubleVec)&spec_userdata_;
spec.info = (mjString)&info; spec.info = (mjString)&info;
} }
@@ -3728,7 +3728,7 @@ void mjCTendon::CopyFromSpec() {
material_ = spec_material_; material_ = spec_material_;
userdata_ = spec_userdata_; userdata_ = spec_userdata_;
material = (mjString)&material_; material = (mjString)&material_;
userdata = (mjDouble)&userdata_; userdata = (mjDoubleVec)&userdata_;
} }
@@ -4097,7 +4097,7 @@ void mjCActuator::MakePointerLocal() {
spec.element = (mjElement)this; spec.element = (mjElement)this;
spec.name = (mjString)&name; spec.name = (mjString)&name;
spec.classname = (mjString)&classname; spec.classname = (mjString)&classname;
spec.userdata = (mjDouble)&spec_userdata_; spec.userdata = (mjDoubleVec)&spec_userdata_;
spec.target = (mjString)&spec_target_; spec.target = (mjString)&spec_target_;
spec.refsite = (mjString)&spec_refsite_; spec.refsite = (mjString)&spec_refsite_;
spec.slidersite = (mjString)&spec_slidersite_; spec.slidersite = (mjString)&spec_slidersite_;
@@ -4114,7 +4114,7 @@ void mjCActuator::CopyFromSpec() {
target_ = spec_target_; target_ = spec_target_;
refsite_ = spec_refsite_; refsite_ = spec_refsite_;
slidersite_ = spec_slidersite_; slidersite_ = spec_slidersite_;
userdata = (mjDouble)&userdata_; userdata = (mjDoubleVec)&userdata_;
target = (mjString)&target_; target = (mjString)&target_;
refsite = (mjString)&refsite_; refsite = (mjString)&refsite_;
slidersite = (mjString)&slidersite_; slidersite = (mjString)&slidersite_;
@@ -4359,7 +4359,7 @@ void mjCSensor::MakePointerLocal() {
spec.element = (mjElement)this; spec.element = (mjElement)this;
spec.name = (mjString)&name; spec.name = (mjString)&name;
spec.classname = (mjString)&classname; spec.classname = (mjString)&classname;
spec.userdata = (mjDouble)&spec_userdata_; spec.userdata = (mjDoubleVec)&spec_userdata_;
spec.objname = (mjString)&spec_objname_; spec.objname = (mjString)&spec_objname_;
spec.refname = (mjString)&spec_refname_; spec.refname = (mjString)&spec_refname_;
spec.plugin.name = (mjString)&plugin_name; spec.plugin.name = (mjString)&plugin_name;
@@ -4374,7 +4374,7 @@ void mjCSensor::CopyFromSpec() {
userdata_ = spec_userdata_; userdata_ = spec_userdata_;
objname_ = spec_objname_; objname_ = spec_objname_;
refname_ = spec_refname_; refname_ = spec_refname_;
userdata = (mjDouble)&userdata_; userdata = (mjDoubleVec)&userdata_;
objname = (mjString)&objname_; objname = (mjString)&objname_;
refname = (mjString)&refname_; refname = (mjString)&refname_;
plugin.active = spec.plugin.active; plugin.active = spec.plugin.active;
+29 -33
View File
@@ -566,7 +566,7 @@ class mjCLight : public mjCBase, private mjmLight {
//------------------------- class mjCFlex ---------------------------------------------------------- //------------------------- class mjCFlex ----------------------------------------------------------
// Describes a flex // Describes a flex
class mjCFlex: public mjCBase { class mjCFlex: public mjCBase, private mjmFlex {
friend class mjCDef; friend class mjCDef;
friend class mjCModel; friend class mjCModel;
friend class mjCFlexcomp; friend class mjCFlexcomp;
@@ -574,41 +574,24 @@ class mjCFlex: public mjCBase {
friend class mjXWriter; friend class mjXWriter;
public: public:
void set_material(std::string _material) { material_ = _material; } mjmFlex spec;
std::string& get_material() { return material_; } using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
// contact properties void CopyFromSpec(void);
int contype; // contact type void PointToLocal(void);
int conaffinity; // contact affinity
int condim; // contact dimensionality
int priority; // contact priority
double friction[3]; // one-sided friction coefficients: slide, roll, spin
double solmix; // solver mixing for contact pairs
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
// other properties // used by mjXWriter and mjCModel
int dim; // element dimensionality const std::string& get_material() { return material_; }
double radius; // radius around primitive element const std::vector<std::string>& get_vertbody() { return vertbody_; }
bool internal; // enable internal collisions const std::vector<double>& get_vert() { return vert_; }
bool flatskin; // render flex skin with flat shading const std::vector<double>& get_elemaabb() { return elemaabb_; }
int selfcollide; // mode for flex self colllision const std::vector<int>& get_elem() { return elem_; }
int activelayers; // number of active element layers in 3D const std::vector<float>& get_texcoord() { return texcoord_; }
int group; // group for visualizatioh
double edgestiffness; // edge stiffness
double edgedamping; // edge damping
float rgba[4]; // rgba when material is omitted
std::vector<std::string> vertbody; // vertex body names bool HasTexcoord() const; // texcoord not null
std::vector<mjtNum> vert; // vertex positions void DelTexcoord(); // delete texcoord
std::vector<mjtNum> elemaabb; // element bounding volume
std::vector<int> elem; // element vertex ids
std::vector<float> texcoord; // vertex texture coordinates
bool HasTexcoord() const; // texcoord not null
void DelTexcoord(); // delete texcoord
private: private:
mjCFlex(mjCModel* = 0); mjCFlex(mjCModel* = 0);
@@ -629,7 +612,20 @@ class mjCFlex: public mjCBase {
std::vector<int> evpair; // element-vertex pairs std::vector<int> evpair; // element-vertex pairs
std::vector<mjtNum> vertxpos; // global vertex positions std::vector<mjtNum> vertxpos; // global vertex positions
mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy
std::vector<mjtNum> elemaabb_; // element bounding volume
// variable-size data
std::vector<std::string> vertbody_; // vertex body names
std::vector<mjtNum> vert_; // vertex positions
std::vector<int> elem_; // element vertex ids
std::vector<float> texcoord_; // vertex texture coordinates
std::string material_; // name of material used for rendering std::string material_; // name of material used for rendering
std::string spec_material_;
std::vector<std::string> spec_vertbody_;
std::vector<mjtNum> spec_vert_;
std::vector<int> spec_elem_;
std::vector<float> spec_texcoord_;
}; };
+52 -33
View File
@@ -1275,14 +1275,22 @@ void mjXReader::Statistic(XMLElement* section) {
//---------------------------------- one-element parsers ------------------------------------------- //---------------------------------- one-element parsers -------------------------------------------
// flex element parser // flex element parser
void mjXReader::OneFlex(XMLElement* elem, mjCFlex* pflex) { void mjXReader::OneFlex(XMLElement* elem, mjmFlex* pflex) {
string text; string text, name, classname, material;
int n; int n;
// read attributes // read attributes
ReadAttrTxt(elem, "name", pflex->name); if (ReadAttrTxt(elem, "name", name)) {
mjm_setString(pflex->name, name.c_str());
}
if (ReadAttrTxt(elem, "classname", classname)) {
mjm_setString(pflex->classname, classname.c_str());
}
if (ReadAttrTxt(elem, "material", material)) {
mjm_setString(pflex->material, material.c_str());
}
ReadAttr(elem, "radius", 1, &pflex->radius, text); ReadAttr(elem, "radius", 1, &pflex->radius, text);
ReadAttrTxt(elem, "material", pflex->get_material());
ReadAttr(elem, "rgba", 4, pflex->rgba, text); ReadAttr(elem, "rgba", 4, pflex->rgba, text);
if (MapValue(elem, "flatskin", &n, bool_map, 2)) { if (MapValue(elem, "flatskin", &n, bool_map, 2)) {
pflex->flatskin = (n==1); pflex->flatskin = (n==1);
@@ -1292,16 +1300,22 @@ void mjXReader::OneFlex(XMLElement* elem, mjCFlex* pflex) {
// read data vectors // read data vectors
if (ReadAttrTxt(elem, "body", text, true)) { if (ReadAttrTxt(elem, "body", text, true)) {
String2Vector(text, pflex->vertbody); mjm_setStringVec(pflex->vertbody, text.c_str());
} }
if (ReadAttrTxt(elem, "vertex", text)) { if (ReadAttrTxt(elem, "vertex", text)) {
String2Vector(text, pflex->vert); std::vector<double> vert;
String2Vector(text, vert);
mjm_setDouble(pflex->vert, vert.data(), vert.size());
} }
if (ReadAttrTxt(elem, "element", text, true)) { if (ReadAttrTxt(elem, "element", text, true)) {
String2Vector(text, pflex->elem); std::vector<int> elem;
String2Vector(text, elem);
mjm_setInt(pflex->elem, elem.data(), elem.size());
} }
if (ReadAttrTxt(elem, "texcoord", text)) { if (ReadAttrTxt(elem, "texcoord", text)) {
String2Vector(text, pflex->texcoord); std::vector<float> texcoord;
String2Vector(text, texcoord);
mjm_setFloat(pflex->texcoord, texcoord.data(), texcoord.size());
} }
// contact subelement // contact subelement
@@ -1331,7 +1345,9 @@ void mjXReader::OneFlex(XMLElement* elem, mjCFlex* pflex) {
ReadAttr(edge, "damping", 1, &pflex->edgedamping, text); ReadAttr(edge, "damping", 1, &pflex->edgedamping, text);
} }
GetXMLPos(elem, pflex); // write error info
mjm_setString(pflex->info,
std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str());
} }
@@ -2400,7 +2416,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjCDef* def) {
// make flexcomp // make flexcomp
void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) { void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) {
string text; string text, material;
int n; int n;
// create out-of-DOM element // create out-of-DOM element
@@ -2417,14 +2433,16 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) {
ReadAttr(elem, "mass", 1, &fcomp.mass, text); ReadAttr(elem, "mass", 1, &fcomp.mass, text);
ReadAttr(elem, "inertiabox", 1, &fcomp.inertiabox, text); ReadAttr(elem, "inertiabox", 1, &fcomp.inertiabox, text);
ReadAttrTxt(elem, "file", fcomp.file); ReadAttrTxt(elem, "file", fcomp.file);
ReadAttrTxt(elem, "material", fcomp.def.flex.get_material()); if (ReadAttrTxt(elem, "material", material)) {
ReadAttr(elem, "rgba", 4, fcomp.def.flex.rgba, text); mjm_setString(fcomp.def.flex.spec.material, material.c_str());
if (MapValue(elem, "flatskin", &n, bool_map, 2)) {
fcomp.def.flex.flatskin = (n==1);
} }
ReadAttrInt(elem, "dim", &fcomp.def.flex.dim); ReadAttr(elem, "rgba", 4, fcomp.def.flex.spec.rgba, text);
ReadAttr(elem, "radius", 1, &fcomp.def.flex.radius, text); if (MapValue(elem, "flatskin", &n, bool_map, 2)) {
ReadAttrInt(elem, "group", &fcomp.def.flex.group); fcomp.def.flex.spec.flatskin = (n==1);
}
ReadAttrInt(elem, "dim", &fcomp.def.flex.spec.dim);
ReadAttr(elem, "radius", 1, &fcomp.def.flex.spec.radius, text);
ReadAttrInt(elem, "group", &fcomp.def.flex.spec.group);
// pose // pose
ReadAttr(elem, "pos", 3, fcomp.pos, text); ReadAttr(elem, "pos", 3, fcomp.pos, text);
@@ -2453,28 +2471,28 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) {
} }
ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.spec.solref, text, false, false); ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.spec.solref, text, false, false);
ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.spec.solimp, text, false, false); ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.spec.solimp, text, false, false);
ReadAttr(edge, "stiffness", 1, &fcomp.def.flex.edgestiffness, text); ReadAttr(edge, "stiffness", 1, &fcomp.def.flex.spec.edgestiffness, text);
ReadAttr(edge, "damping", 1, &fcomp.def.flex.edgedamping, text); ReadAttr(edge, "damping", 1, &fcomp.def.flex.spec.edgedamping, text);
} }
// contact // contact
XMLElement* cont = FirstChildElement(elem, "contact"); XMLElement* cont = FirstChildElement(elem, "contact");
if (cont) { if (cont) {
ReadAttrInt(cont, "contype", &fcomp.def.flex.contype); ReadAttrInt(cont, "contype", &fcomp.def.flex.spec.contype);
ReadAttrInt(cont, "conaffinity", &fcomp.def.flex.conaffinity); ReadAttrInt(cont, "conaffinity", &fcomp.def.flex.spec.conaffinity);
ReadAttrInt(cont, "condim", &fcomp.def.flex.condim); ReadAttrInt(cont, "condim", &fcomp.def.flex.spec.condim);
ReadAttrInt(cont, "priority", &fcomp.def.flex.priority); ReadAttrInt(cont, "priority", &fcomp.def.flex.spec.priority);
ReadAttr(cont, "friction", 3, fcomp.def.flex.friction, text, false, false); ReadAttr(cont, "friction", 3, fcomp.def.flex.spec.friction, text, false, false);
ReadAttr(cont, "solmix", 1, &fcomp.def.flex.solmix, text); ReadAttr(cont, "solmix", 1, &fcomp.def.flex.spec.solmix, text);
ReadAttr(cont, "solref", mjNREF, fcomp.def.flex.solref, text, false, false); ReadAttr(cont, "solref", mjNREF, fcomp.def.flex.spec.solref, text, false, false);
ReadAttr(cont, "solimp", mjNIMP, fcomp.def.flex.solimp, text, false, false); ReadAttr(cont, "solimp", mjNIMP, fcomp.def.flex.spec.solimp, text, false, false);
ReadAttr(cont, "margin", 1, &fcomp.def.flex.margin, text); ReadAttr(cont, "margin", 1, &fcomp.def.flex.spec.margin, text);
ReadAttr(cont, "gap", 1, &fcomp.def.flex.gap, text); ReadAttr(cont, "gap", 1, &fcomp.def.flex.spec.gap, text);
if (MapValue(cont, "internal", &n, bool_map, 2)) { if (MapValue(cont, "internal", &n, bool_map, 2)) {
fcomp.def.flex.internal = (n==1); fcomp.def.flex.spec.internal = (n==1);
} }
MapValue(cont, "selfcollide", &fcomp.def.flex.selfcollide, flexself_map, 5); MapValue(cont, "selfcollide", &fcomp.def.flex.spec.selfcollide, flexself_map, 5);
ReadAttrInt(cont, "activelayers", &fcomp.def.flex.activelayers); ReadAttrInt(cont, "activelayers", &fcomp.def.flex.spec.activelayers);
} }
// pin // pin
@@ -2639,6 +2657,7 @@ void mjXReader::Default(XMLElement* section, int parentid) {
mjm_finalize(def->material.spec.element); mjm_finalize(def->material.spec.element);
mjm_finalize(def->equality.spec.element); mjm_finalize(def->equality.spec.element);
mjm_finalize(def->tendon.spec.element); mjm_finalize(def->tendon.spec.element);
mjm_finalize(def->flex.spec.element);
// advance // advance
elem = NextSiblingElement(elem); elem = NextSiblingElement(elem);
@@ -3385,7 +3404,7 @@ void mjXReader::Deformable(XMLElement* section) {
// flex sub-element // flex sub-element
if (name=="flex") { if (name=="flex") {
// create flex and parse // create flex and parse
mjCFlex* pflex = model->AddFlex(); mjmFlex* pflex = mjm_addFlex(model);
OneFlex(elem, pflex); OneFlex(elem, pflex);
} }
+1 -1
View File
@@ -53,7 +53,7 @@ class mjXReader : public mjXBase {
void Keyframe(tinyxml2::XMLElement* section); // keyframe section void Keyframe(tinyxml2::XMLElement* section); // keyframe section
// single element parsers, used in defaults and main body // single element parsers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex); void OneFlex(tinyxml2::XMLElement* elem, mjmFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh); void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh);
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin); void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjmMaterial* pmaterial); void OneMaterial(tinyxml2::XMLElement* elem, mjmMaterial* pmaterial);
+8 -8
View File
@@ -100,20 +100,20 @@ void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) {
WriteAttrInt(elem, "group", pflex->group, defflex.group); WriteAttrInt(elem, "group", pflex->group, defflex.group);
// data vectors // data vectors
if (!pflex->vertbody.empty()) { if (!pflex->get_vertbody().empty()) {
Vector2String(text, pflex->vertbody); Vector2String(text, pflex->get_vertbody());
WriteAttrTxt(elem, "body", text); WriteAttrTxt(elem, "body", text);
} }
if (!pflex->vert.empty()) { if (!pflex->get_vert().empty()) {
Vector2String(text, pflex->vert); Vector2String(text, pflex->get_vert());
WriteAttrTxt(elem, "vertex", text); WriteAttrTxt(elem, "vertex", text);
} }
if (!pflex->elem.empty()) { if (!pflex->get_elem().empty()) {
Vector2String(text, pflex->elem); Vector2String(text, pflex->get_elem());
WriteAttrTxt(elem, "element", text); WriteAttrTxt(elem, "element", text);
} }
if (!pflex->texcoord.empty()) { if (!pflex->get_texcoord().empty()) {
Vector2String(text, pflex->texcoord); Vector2String(text, pflex->get_texcoord());
WriteAttrTxt(elem, "texcoord", text); WriteAttrTxt(elem, "texcoord", text);
} }