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

Add copy constructor and assignment to mjCModel.

Browse Source 
PiperOrigin-RevId: 616850780
Change-Id: I7fedab2efa2a90fd7ce952eac4e195f77c5ff5bc
This commit is contained in:
Alessio Quaglino
2024-03-18 09:24:36 -07:00
committed by Copybara-Service
parent de918a8b2d
commit 372aa021d0
8 changed files with 766 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+8
View File
@@ -36,6 +36,14 @@ mjSpec* mjm_createSpec() {
// copy model
mjSpec* mjm_copySpec(const mjSpec* s) {
mjCModel* modelC = new mjCModel(*reinterpret_cast<mjCModel*>(s->element));
return &modelC->spec;
}
// copy back model
void mjm_copyBack(mjSpec* s, const mjModel* m) {
mjCModel* modelC = reinterpret_cast<mjCModel*>(s->element);
src/user/user_api.h
+3
View File
@@ -731,6 +731,9 @@ typedef struct _mjmDefault { // default specification
// Create model.
MJAPI mjSpec* mjm_createSpec();
// Copy model.
MJAPI mjSpec* mjm_copySpec(const mjSpec* s);
// Copy back model.
MJAPI void mjm_copyBack(mjSpec* s, const mjModel* m);
src/user/user_mesh.cc
+39
View File
@@ -200,41 +200,62 @@ mjCMesh& mjCMesh::operator=(const mjCMesh& other) {
size_t nvert = 3*other.nvert_*sizeof(float);
this->vert_ = (float*)mju_malloc(nvert);
memcpy(this->vert_, other.vert_, nvert);
} else {
this->vert_ = NULL;
}
if (other.normal_) {
size_t nnormal = 3*other.nnormal_*sizeof(float);
this->normal_ = (float*)mju_malloc(nnormal);
memcpy(this->normal_, other.normal_, nnormal);
} else {
this->normal_ = NULL;
}
if (other.center_) {
size_t ncenter = 3*other.nface_*sizeof(double);
this->center_ = (double*)mju_malloc(ncenter);
memcpy(this->center_, other.center_, ncenter);
} else {
this->center_ = NULL;
}
if (other.texcoord_) {
size_t ntexcoord = 2*other.ntexcoord_*sizeof(float);
this->texcoord_ = (float*)mju_malloc(ntexcoord);
memcpy(this->texcoord_, other.texcoord_, ntexcoord);
} else {
this->texcoord_ = NULL;
}
if (other.face_) {
size_t nface = 3*other.nface_*sizeof(int);
this->face_ = (int*)mju_malloc(nface);
memcpy(this->face_, other.face_, nface);
} else {
this->face_ = NULL;
}
if (other.facenormal_) {
size_t nfacenormal = 3*other.nface_*sizeof(int);
this->facenormal_ = (int*)mju_malloc(nfacenormal);
memcpy(this->facenormal_, other.facenormal_, nfacenormal);
} else {
this->facenormal_ = NULL;
}
if (other.facetexcoord_) {
size_t nfacetexcoord = 3*other.nface_*sizeof(int);
this->facetexcoord_ = (int*)mju_malloc(nfacetexcoord);
memcpy(this->facetexcoord_, other.facetexcoord_, nfacetexcoord);
} else {
this->facetexcoord_ = NULL;
}
if (other.graph_) {
size_t szgraph = szgraph_*sizeof(int);
this->graph_ = (int*)mju_malloc(szgraph);
memcpy(this->graph_, other.graph_, szgraph);
} else {
this->graph_ = NULL;
}
if (other.plugin.instance) {
mjCPlugin* new_plugin = new mjCPlugin(*reinterpret_cast<mjCPlugin*>(other.plugin.instance));
plugin = new_plugin->spec;
model->plugins.push_back(new_plugin);
}
}
PointToLocal();
@@ -2061,6 +2082,24 @@ mjCSkin::mjCSkin(mjCModel* _model) {
mjCSkin::mjCSkin(const mjCSkin& other) {
*this = other;
}
mjCSkin& mjCSkin::operator=(const mjCSkin& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCSkin_*>(this) = static_cast<const mjCSkin_&>(other);
*static_cast<mjmSkin*>(this) = static_cast<const mjmSkin&>(other);
}
PointToLocal();
return *this;
}
void mjCSkin::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
src/user/user_model.cc
+175 -22
View File
@@ -138,6 +138,175 @@ mjCModel::mjCModel() {
world->def = defaults[0];
bodies.push_back(world);
// create mjCBase lists from children lists
CreateObjectLists();
// point to model from spec
PointToLocal();
// this class allocated the plugins
plugin_owner = true;
}
mjCModel::mjCModel(const mjCModel& other) {
*this = other;
}
mjCModel& mjCModel::operator=(const mjCModel& other) {
if (this != &other) {
plugin_owner = false;
this->spec = other.spec;
*static_cast<mjCModel_*>(this) = static_cast<const mjCModel_&>(other);
*static_cast<mjSpec*>(this) = static_cast<const mjSpec&>(other);
std::map<mjCDef*, int> def_map;
for (int i = 0; i < other.defaults.size(); i++) {
defaults.push_back(new mjCDef(*other.defaults[i]));
def_map[other.defaults[i]] = i;
}
for (const mjCFlex* flex : other.flexes) {
flexes.push_back(new mjCFlex(*flex));
flexes.back()->model = this;
flexes.back()->def = defaults[def_map[flex->def]];
}
for (const mjCMesh* mesh : other.meshes) {
meshes.push_back(new mjCMesh(*mesh));
meshes.back()->model = this;
meshes.back()->def = defaults[def_map[mesh->def]];
}
for (const mjCSkin* skin : other.skins) {
skins.push_back(new mjCSkin(*skin));
skins.back()->model = this;
skins.back()->def = defaults[def_map[skin->def]];
}
for (const mjCHField* hfield : other.hfields) {
hfields.push_back(new mjCHField(*hfield));
hfields.back()->model = this;
hfields.back()->def = defaults[def_map[hfield->def]];
}
for (const mjCTexture* texture : other.textures) {
textures.push_back(new mjCTexture(*texture));
textures.back()->model = this;
textures.back()->def = defaults[def_map[texture->def]];
}
for (const mjCMaterial* material : other.materials) {
materials.push_back(new mjCMaterial(*material));
materials.back()->model = this;
materials.back()->def = defaults[def_map[material->def]];
}
for (const mjCPair* pair : other.pairs) {
pairs.push_back(new mjCPair(*pair));
pairs.back()->model = this;
pairs.back()->def = defaults[def_map[pair->def]];
}
for (const mjCBodyPair* exclude : other.excludes) {
excludes.push_back(new mjCBodyPair(*exclude));
excludes.back()->model = this;
excludes.back()->def = defaults[def_map[exclude->def]];
}
for (const mjCEquality* equality : other.equalities) {
equalities.push_back(new mjCEquality(*equality));
equalities.back()->model = this;
equalities.back()->def = defaults[def_map[equality->def]];
}
for (const mjCTendon* tendon : other.tendons) {
tendons.push_back(new mjCTendon(*tendon));
tendons.back()->SetModel(this);
tendons.back()->def = defaults[def_map[tendon->def]];
}
for (const mjCActuator* actuator : other.actuators) {
actuators.push_back(new mjCActuator(*actuator));
actuators.back()->model = this;
actuators.back()->def = defaults[def_map[actuator->def]];
}
for (const mjCSensor* sensor : other.sensors) {
sensors.push_back(new mjCSensor(*sensor));
sensors.back()->model = this;
sensors.back()->def = defaults[def_map[sensor->def]];
}
for (const mjCNumeric* numeric : other.numerics) {
numerics.push_back(new mjCNumeric(*numeric));
numerics.back()->model = this;
numerics.back()->def = defaults[def_map[numeric->def]];
}
for (const mjCText* text : other.texts) {
texts.push_back(new mjCText(*text));
texts.back()->model = this;
texts.back()->def = defaults[def_map[text->def]];
}
for (const mjCTuple* tuple : other.tuples) {
tuples.push_back(new mjCTuple(*tuple));
tuples.back()->model = this;
tuples.back()->def = defaults[def_map[tuple->def]];
}
for (const mjCKey* key : other.keys) {
keys.push_back(new mjCKey(*key));
keys.back()->model = this;
keys.back()->def = defaults[def_map[key->def]];
}
// plugins are global
plugins = other.plugins;
active_plugins = other.active_plugins;
// the world copy constructor takes care of copying the tree
mjCBody* world = new mjCBody(*other.bodies[0], this);
// create global lists
bodies.push_back(world);
MakeLists(bodies[0]);
// update defaults for the copied objects
for (int i = 1; i < other.bodies.size(); i++) {
bodies[i]->def = defaults[def_map[other.bodies[i]->def]];
}
for (int i = 0; i < other.joints.size(); i++) {
joints[i]->def = defaults[def_map[other.joints[i]->def]];
}
for (int i = 0; i < other.geoms.size(); i++) {
geoms[i]->def = defaults[def_map[other.geoms[i]->def]];
}
for (int i = 0; i < other.sites.size(); i++) {
sites[i]->def = defaults[def_map[other.sites[i]->def]];
}
for (int i = 0; i < other.cameras.size(); i++) {
cameras[i]->def = defaults[def_map[other.cameras[i]->def]];
}
for (int i = 0; i < other.lights.size(); i++) {
lights[i]->def = defaults[def_map[other.lights[i]->def]];
}
// copy name maps
for (int i=0; i<mjNOBJECT; i++) {
ids[i] = other.ids[i];
}
// cast children to mjCBase
CreateObjectLists();
// restore to the same state as other
if (!compiled) {
bodies.clear();
frames.clear();
joints.clear();
geoms.clear();
sites.clear();
cameras.clear();
lights.clear();
bodies.push_back(world);
}
}
PointToLocal();
return *this;
}
// TODO: we should not use C-type casting with multiple C++ inheritance
void mjCModel::CreateObjectLists() {
for (int i = 0; i < mjNOBJECT; ++i) {
object_lists[i] = nullptr;
}
@@ -166,17 +335,6 @@ mjCModel::mjCModel() {
object_lists[mjOBJ_TUPLE] = (std::vector<mjCBase*>*) &tuples;
object_lists[mjOBJ_KEY] = (std::vector<mjCBase*>*) &keys;
object_lists[mjOBJ_PLUGIN] = (std::vector<mjCBase*>*) &plugins;
// point to model from spec
PointToLocal();
}
mjCModel::mjCModel(const mjCModel& other) {
*this = other;
PointToLocal();
}
@@ -230,9 +388,12 @@ mjCModel::~mjCModel() {
for (int i=0; i<texts.size(); i++) delete texts[i];
for (int i=0; i<tuples.size(); i++) delete tuples[i];
for (int i=0; i<keys.size(); i++) delete keys[i];
for (int i=0; i<plugins.size(); i++) delete plugins[i];
for (int i=0; i<defaults.size(); i++) delete defaults[i];
if (plugin_owner) {
for (int i=0; i<plugins.size(); i++) delete plugins[i];
}
// clear pointer lists created in model construction
flexes.clear();
meshes.clear();
@@ -339,7 +500,6 @@ void mjCModel::Clear(void) {
hasImplicitPluginElem = false;
compiled = false;
errInfo = mjCError();
fixCount = 0;
qpos0.clear();
}
@@ -504,13 +664,6 @@ bool mjCModel::IsCompiled(void) {
// number of massless bodies that were fixed
int mjCModel::GetFixed(void) {
return fixCount;
}
// get reference of error object
const mjCError& mjCModel::GetError(void) {
return errInfo;
@@ -2144,7 +2297,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->hfield_adr[i] = data_adr;
// copy elevation data
memcpy(m->hfield_data + data_adr, phf->data, phf->nrow*phf->ncol*sizeof(float));
memcpy(m->hfield_data + data_adr, phf->data.data(), phf->nrow*phf->ncol*sizeof(float));
// advance counter
data_adr += phf->nrow*phf->ncol;
@@ -2163,7 +2316,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->tex_adr[i] = data_adr;
// copy rgb data
memcpy(m->tex_rgb + data_adr, ptex->rgb, 3*ptex->width*ptex->height);
memcpy(m->tex_rgb + data_adr, ptex->rgb.data(), 3*ptex->width*ptex->height);
// advance counter
data_adr += 3*ptex->width*ptex->height;
src/user/user_model.h
+158 -151
View File
@@ -33,133 +33,9 @@
typedef std::map<std::string, int, std::less<> > mjKeyMap;
typedef std::array<mjKeyMap, mjNOBJECT> mjListKeyMap;
// mjCModel contains everything needed to generate the low-level model.
// It can be constructed manually by calling 'Add' functions and setting
// the public fields of the various objects. Alternatively it can constructed
// by loading an XML file via mjCXML. Once an mjCModel object is
// constructed, 'Compile' can be called to generate the corresponding mjModel object
// (which is the low-level model). The mjCModel object can then be deleted.
class mjCModel : private mjSpec {
friend class mjCBody;
friend class mjCCamera;
friend class mjCGeom;
friend class mjCFlex;
friend class mjCHField;
friend class mjCFrame;
friend class mjCJoint;
friend class mjCEquality;
friend class mjCMesh;
friend class mjCSkin;
friend class mjCSite;
friend class mjCTendon;
friend class mjCTexture;
friend class mjCActuator;
friend class mjCSensor;
friend class mjCDef;
friend class mjXReader;
friend class mjXWriter;
public:
mjCModel();
mjCModel(const mjCModel& other);
~mjCModel();
void CopyFromSpec(); // copy spec to private attributes
void PointToLocal();
mjSpec spec;
mjModel* Compile(const mjVFS* vfs = nullptr); // construct mjModel
bool CopyBack(const mjModel*); // DECOMPILER: copy numeric back
void FuseStatic(); // fuse static bodies with parent
void FuseReindex(mjCBody* body); // reindex elements during fuse
// API for adding model elements
mjCFlex* AddFlex();
mjCMesh* AddMesh(mjCDef* def = nullptr);
mjCSkin* AddSkin();
mjCHField* AddHField();
mjCTexture* AddTexture();
mjCMaterial* AddMaterial(mjCDef* def = nullptr);
mjCPair* AddPair(mjCDef* def = nullptr); // geom pair for inclusion
mjCBodyPair* AddExclude(); // body pair for exclusion
mjCEquality* AddEquality(mjCDef* def = nullptr); // equality constraint
mjCTendon* AddTendon(mjCDef* def = nullptr);
mjCActuator* AddActuator(mjCDef* def = nullptr);
mjCSensor* AddSensor();
mjCNumeric* AddNumeric();
mjCText* AddText();
mjCTuple* AddTuple();
mjCKey* AddKey();
mjCPlugin* AddPlugin();
// delete elements marked as discard=true
template <class T> void Delete(std::vector<T*>& elements,
const std::vector<bool>& discard);
// delete all elements
template <class T> void DeleteAll(std::vector<T*>& elements);
// API for access to model elements (outside tree)
int NumObjects(mjtObj type); // number of objects in specified list
mjCBase* GetObject(mjtObj type, int id); // pointer to specified object
// API for access to other variables
bool IsCompiled(); // is model already compiled
int GetFixed(); // number of fixed massless bodies
const mjCError& GetError(void); // get reference of error object
mjCBody* GetWorld(); // pointer to world body
mjCDef* FindDef(std::string name); // find default class name
mjCDef* AddDef(std::string name, int parentid); // add default class to array
mjCBase* FindObject(mjtObj type, std::string name); // find object given type and name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat); // detect null pose
// accessors
std::string get_meshdir(void) const { return meshdir_; }
std::string get_texturedir(void) const { return texturedir_; }
// resolve plugin instance, create a new one if needed
void ResolvePlugin(mjCBase* obj, const std::string& plugin_name,
const std::string& plugin_instance_name,
mjCPlugin** plugin_instance);
// settings for each defaults class
std::vector<mjCDef*> defaults;
// list of active plugins
std::vector<std::pair<const mjpPlugin*, int>> active_plugins;
private:
void TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs);
mjModel* _Compile(const mjVFS* vfs);
// clear objects allocated by Compile
void Clear(void);
// add object of any type
template <class T> T* AddObject(std::vector<T*>& list, std::string type);
// add object of any type, with def parameter
template <class T> T* AddObjectDef(std::vector<T*>& list, std::string type,
mjCDef* def);
// if asset name is missing, set to filename
template<class T> void SetDefaultNames(std::vector<T*>& assets);
// delete material from object
template <class T> void DeleteMaterial(std::vector<T*>& list,
std::string_view name = "");
// compile phases
void MakeLists(mjCBody* body); // make lists of bodies, geoms, joints, sites
void IndexAssets(bool discard); // convert asset names into indices
void CheckEmptyNames(); // check empty names
void SetSizes(); // compute sizes
void AutoSpringDamper(mjModel*); // automatic stiffness and damping computation
void LengthRange(mjModel*, mjData*); // compute actuator lengthrange
void CopyNames(mjModel*); // copy names, compute name addresses
void CopyPaths(mjModel*); // copy paths, compute path addresses
void CopyObjects(mjModel*); // copy objects outside kinematic tree
void CopyTree(mjModel*); // copy objects inside kinematic tree
class mjCModel_ {
protected:
bool compiled; // already compiled flag
// sizes set from object list lengths
int nbody; // number of bodies
@@ -224,6 +100,158 @@ class mjCModel : private mjSpec {
int nD; // number of non-zeros in sparse dof-dof matrix
int nB; // number of non-zeros in sparse body-dof matrix
// statistics, as computed by mj_setConst
double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst
double meanmass_auto; // mean body mass, as computed by mj_setConst
double meansize_auto; // mean body size, as computed by mj_setConst
double extent_auto; // spatial extent, as computed by mj_setConst
double center_auto[3]; // center of model, as computed by mj_setConst
// save qpos0, to recognize changed key_qpos in write
std::vector<mjtNum> qpos0;
// variable-size attributes
std::string comment_; // comment at top of XML
std::string modelfiledir_; // path to model file
std::string modelname_;
std::string meshdir_;
std::string texturedir_;
std::string spec_comment_;
std::string spec_modelfiledir_;
std::string spec_modelname_;
std::string spec_meshdir_;
std::string spec_texturedir_;
};
// mjCModel contains everything needed to generate the low-level model.
// It can be constructed manually by calling 'Add' functions and setting
// the public fields of the various objects. Alternatively it can constructed
// by loading an XML file via mjCXML. Once an mjCModel object is
// constructed, 'Compile' can be called to generate the corresponding mjModel object
// (which is the low-level model). The mjCModel object can then be deleted.
class mjCModel : public mjCModel_, private mjSpec {
friend class mjCBase;
friend class mjCBody;
friend class mjCCamera;
friend class mjCGeom;
friend class mjCFlex;
friend class mjCHField;
friend class mjCFrame;
friend class mjCJoint;
friend class mjCEquality;
friend class mjCMesh;
friend class mjCSkin;
friend class mjCSite;
friend class mjCTendon;
friend class mjCTexture;
friend class mjCActuator;
friend class mjCSensor;
friend class mjCDef;
friend class mjXReader;
friend class mjXWriter;
public:
mjCModel();
mjCModel(const mjCModel& other);
mjCModel& operator=(const mjCModel& other);
~mjCModel();
void CopyFromSpec(); // copy spec to private attributes
void PointToLocal();
mjSpec spec;
mjModel* Compile(const mjVFS* vfs = nullptr); // construct mjModel
bool CopyBack(const mjModel*); // DECOMPILER: copy numeric back
void FuseStatic(); // fuse static bodies with parent
void FuseReindex(mjCBody* body); // reindex elements during fuse
// API for adding model elements
mjCFlex* AddFlex();
mjCMesh* AddMesh(mjCDef* def = nullptr);
mjCSkin* AddSkin();
mjCHField* AddHField();
mjCTexture* AddTexture();
mjCMaterial* AddMaterial(mjCDef* def = nullptr);
mjCPair* AddPair(mjCDef* def = nullptr); // geom pair for inclusion
mjCBodyPair* AddExclude(); // body pair for exclusion
mjCEquality* AddEquality(mjCDef* def = nullptr); // equality constraint
mjCTendon* AddTendon(mjCDef* def = nullptr);
mjCActuator* AddActuator(mjCDef* def = nullptr);
mjCSensor* AddSensor();
mjCNumeric* AddNumeric();
mjCText* AddText();
mjCTuple* AddTuple();
mjCKey* AddKey();
mjCPlugin* AddPlugin();
// delete elements marked as discard=true
template <class T> void Delete(std::vector<T*>& elements,
const std::vector<bool>& discard);
// delete all elements
template <class T> void DeleteAll(std::vector<T*>& elements);
// API for access to model elements (outside tree)
int NumObjects(mjtObj type); // number of objects in specified list
mjCBase* GetObject(mjtObj type, int id); // pointer to specified object
// API for access to other variables
bool IsCompiled(); // is model already compiled
const mjCError& GetError(void); // get reference of error object
mjCBody* GetWorld(); // pointer to world body
mjCDef* FindDef(std::string name); // find default class name
mjCDef* AddDef(std::string name, int parentid); // add default class to array
mjCBase* FindObject(mjtObj type, std::string name); // find object given type and name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat); // detect null pose
// accessors
std::string get_meshdir(void) const { return meshdir_; }
std::string get_texturedir(void) const { return texturedir_; }
// resolve plugin instance, create a new one if needed
void ResolvePlugin(mjCBase* obj, const std::string& plugin_name,
const std::string& plugin_instance_name,
mjCPlugin** plugin_instance);
// settings for each defaults class
std::vector<mjCDef*> defaults;
// list of active plugins
std::vector<std::pair<const mjpPlugin*, int>> active_plugins;
private:
void TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs);
mjModel* _Compile(const mjVFS* vfs);
// clear objects allocated by Compile
void Clear(void);
// add object of any type
template <class T> T* AddObject(std::vector<T*>& list, std::string type);
// add object of any type, with def parameter
template <class T> T* AddObjectDef(std::vector<T*>& list, std::string type,
mjCDef* def);
// if asset name is missing, set to filename
template<class T> void SetDefaultNames(std::vector<T*>& assets);
// delete material from object
template <class T> void DeleteMaterial(std::vector<T*>& list,
std::string_view name = "");
// compile phases
void MakeLists(mjCBody* body); // make lists of bodies, geoms, joints, sites
void IndexAssets(bool discard); // convert asset names into indices
void CheckEmptyNames(); // check empty names
void SetSizes(); // compute sizes
void AutoSpringDamper(mjModel*); // automatic stiffness and damping computation
void LengthRange(mjModel*, mjData*); // compute actuator lengthrange
void CopyNames(mjModel*); // copy names, compute name addresses
void CopyPaths(mjModel*); // copy paths, compute path addresses
void CopyObjects(mjModel*); // copy objects outside kinematic tree
void CopyTree(mjModel*); // copy objects inside kinematic tree
// objects created here
std::vector<mjCFlex*> flexes; // list of flexes
std::vector<mjCMesh*> meshes; // list of meshes
@@ -255,32 +283,11 @@ class mjCModel : private mjSpec {
// array of pointers to each object list (enumerated by type)
std::array<std::vector<mjCBase*>*, mjNOBJECT> object_lists;
// statistics, as computed by mj_setConst
double meaninertia_auto; // mean diagonal inertia, as computed by mj_setConst
double meanmass_auto; // mean body mass, as computed by mj_setConst
double meansize_auto; // mean body size, as computed by mj_setConst
double extent_auto; // spatial extent, as computed by mj_setConst
double center_auto[3]; // center of model, as computed by mj_setConst
// create mjCBase lists from children lists
void CreateObjectLists();
mjListKeyMap ids; // map from object names to ids
bool compiled; // already compiled flag (cannot be compiled again)
mjCError errInfo; // last error info
int fixCount; // how many bodies have been fixed
// save qpos0, to recognize changed key_qpos in write
std::vector<mjtNum> qpos0;
// variable-size attributes
std::string comment_; // comment at top of XML
std::string modelfiledir_; // path to model file
std::string modelname_;
std::string meshdir_;
std::string texturedir_;
std::string spec_comment_;
std::string spec_modelfiledir_;
std::string spec_modelname_;
std::string spec_meshdir_;
std::string spec_texturedir_;
bool plugin_owner; // this class allocated the plugins
};
#endif // MUJOCO_SRC_USER_USER_MODEL_H_
src/user/user_objects.cc
+328 -132
View File
@@ -570,12 +570,6 @@ mjCBase::mjCBase(const mjCBase& other) {
mjCBase& mjCBase::operator=(const mjCBase& other) {
if (this != &other) {
*static_cast<mjCBase_*>(this) = static_cast<const mjCBase_&>(other);
if (other.def) {
def = new mjCDef(*other.def);
}
if (other.frame) {
frame = new mjCFrame(*other.frame);
}
}
return *this;
}
@@ -663,7 +657,8 @@ mjCBody::mjCBody(mjCModel* _model) {
mjCBody::mjCBody(const mjCBody& other) {
mjCBody::mjCBody(const mjCBody& other, mjCModel* _model) {
model = _model;
*this = other;
}
@@ -671,43 +666,63 @@ mjCBody::mjCBody(const mjCBody& other) {
mjCBody& mjCBody::operator=(const mjCBody& other) {
if (this != &other) {
this->spec = other.spec;
spec = other.spec;
*static_cast<mjCBody_*>(this) = static_cast<const mjCBody_&>(other);
*static_cast<mjmBody*>(this) = static_cast<const mjmBody&>(other);
this->bodies.clear();
this->frames.clear();
this->geoms.clear();
this->joints.clear();
this->sites.clear();
this->cameras.clear();
this->lights.clear();
std::map<mjCFrame*, int> fmap;
mjCFrame *np = nullptr;
bodies.clear();
frames.clear();
geoms.clear();
joints.clear();
sites.clear();
cameras.clear();
lights.clear();
// create frames and map them to the old ones
for (int i=0; i<other.frames.size(); i++) {
frames.push_back(new mjCFrame(*other.frames[i]));
frames.back()->model = model;
fmap[other.frames[i]] = i;
}
// copy all children
for (int i=0; i<other.bodies.size(); i++) {
this->bodies.push_back(new mjCBody(*other.bodies[i])); // triggers recursive call
bodies.push_back(new mjCBody(*other.bodies[i], model)); // triggers recursive call
bodies.back()->frame = other.bodies[i]->frame ? frames[fmap[other.bodies[i]->frame]] : np;
}
for (int i=0; i<other.frames.size(); i++) {
this->frames.push_back(new mjCFrame(*other.frames[i]));
frames[i]->frame = other.frames[i]->frame ? frames[fmap[other.frames[i]->frame]] : np;
}
for (int i=0; i<other.geoms.size(); i++) {
this->geoms.push_back(new mjCGeom(*other.geoms[i]));
this->geoms.back()->body = this;
geoms.push_back(new mjCGeom(*other.geoms[i]));
geoms.back()->body = this;
geoms.back()->model = model;
geoms.back()->frame = other.geoms[i]->frame ? frames[fmap[other.geoms[i]->frame]] : np;
}
for (int i=0; i<other.joints.size(); i++) {
this->joints.push_back(new mjCJoint(*other.joints[i]));
this->joints.back()->body = this;
joints.push_back(new mjCJoint(*other.joints[i]));
joints.back()->body = this;
joints.back()->model = model;
joints.back()->frame = other.joints[i]->frame ? frames[fmap[other.joints[i]->frame]] : np;
}
for (int i=0; i<other.sites.size(); i++) {
this->sites.push_back(new mjCSite(*other.sites[i]));
this->sites.back()->body = this;
sites.push_back(new mjCSite(*other.sites[i]));
sites.back()->body = this;
sites.back()->model = model;
sites.back()->frame = other.sites[i]->frame ? frames[fmap[other.sites[i]->frame]] : np;
}
for (int i=0; i<other.cameras.size(); i++) {
this->cameras.push_back(new mjCCamera(*other.cameras[i]));
this->cameras.back()->body = this;
cameras.push_back(new mjCCamera(*other.cameras[i]));
cameras.back()->body = this;
cameras.back()->model = model;
cameras.back()->frame = other.cameras[i]->frame ? frames[fmap[other.cameras[i]->frame]] : np;
}
for (int i=0; i<other.lights.size(); i++) {
this->lights.push_back(new mjCLight(*other.lights[i]));
this->lights.back()->body = this;
lights.push_back(new mjCLight(*other.lights[i]));
lights.back()->body = this;
lights.back()->model = model;
lights.back()->frame = other.lights[i]->frame ? frames[fmap[other.lights[i]->frame]] : np;
}
}
PointToLocal();
@@ -2618,7 +2633,7 @@ mjCHField::mjCHField(mjCModel* _model) {
model = _model;
// clear variables
data = 0;
data.clear();
spec_file_.clear();
spec_userdata_.clear();
@@ -2631,6 +2646,24 @@ mjCHField::mjCHField(mjCModel* _model) {
mjCHField::mjCHField(const mjCHField& other) {
*this = other;
}
mjCHField& mjCHField::operator=(const mjCHField& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCHField_*>(this) = static_cast<const mjCHField_&>(other);
*static_cast<mjmHField*>(this) = static_cast<const mjmHField&>(other);
}
PointToLocal();
return *this;
}
void mjCHField::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
@@ -2652,10 +2685,7 @@ void mjCHField::CopyFromSpec() {
userdata = (mjFloatVec)&userdata_;
// clear precompiled asset. TODO: use asset cache
if (data) {
mju_free(data);
data = 0;
}
data.clear();
if (!file_.empty()) {
nrow = 0;
ncol = 0;
@@ -2666,9 +2696,7 @@ void mjCHField::CopyFromSpec() {
// destructor
mjCHField::~mjCHField() {
if (data) {
mju_free(data);
}
data.clear();
userdata_.clear();
spec_userdata_.clear();
}
@@ -2708,13 +2736,13 @@ void mjCHField::LoadCustom(mjResource* resource) {
}
// allocate
data = (float*) mju_malloc(nrow*ncol*sizeof(float));
if (!data) {
data.assign(nrow*ncol, 0);
if (data.empty()) {
throw mjCError(this, "could not allocate buffers in hfield");
}
// copy data
memcpy(data, (void*)(pint+2), nrow*ncol*sizeof(float));
memcpy(data.data(), (void*)(pint+2), nrow*ncol*sizeof(float));
}
@@ -2747,8 +2775,8 @@ void mjCHField::LoadPNG(mjResource* resource) {
}
// allocate
data = (float*) mju_malloc(w*h*sizeof(float));
if (!data) {
data.assign(w*h, 0);
if (data.empty()) {
throw mjCError(this, "could not allocate buffers in hfield");
}
@@ -2770,11 +2798,11 @@ void mjCHField::Compile(const mjVFS* vfs) {
// copy userdata into data
if (!userdata_.empty()) {
data = (float*) mju_malloc(nrow*ncol*sizeof(float));
if (!data) {
data.assign(nrow*ncol, 0);
if (data.empty()) {
throw mjCError(this, "could not allocate buffers in hfield");
}
memcpy(data, userdata_.data(), nrow*ncol*sizeof(float));
memcpy(data.data(), userdata_.data(), nrow*ncol*sizeof(float));
}
// check size parameters
@@ -2791,7 +2819,7 @@ void mjCHField::Compile(const mjVFS* vfs) {
// load from file if specified
if (!file_.empty()) {
// make sure hfield was not already specified manually
if (nrow || ncol || data) {
if (nrow || ncol || !data.empty()) {
throw mjCError(this,
"hfield '%s' (id = %d) specified from file and manually", name.c_str(), id);
}
@@ -2824,7 +2852,7 @@ void mjCHField::Compile(const mjVFS* vfs) {
}
// make sure hfield was specified (from file or manually)
if (nrow<1 || ncol<1 || data==0) {
if (nrow<1 || ncol<1 || data.empty()) {
throw mjCError(this, "hfield '%s' (id = %d) not specified", name.c_str(), id);
}
@@ -2864,7 +2892,7 @@ mjCTexture::mjCTexture(mjCModel* _model) {
spec_cubefiles_.assign(6, "");
// clear internal variables
rgb = 0;
rgb.clear();
// point to local
PointToLocal();
@@ -2875,6 +2903,23 @@ mjCTexture::mjCTexture(mjCModel* _model) {
mjCTexture::mjCTexture(const mjCTexture& other) {
*this = other;
}
mjCTexture& mjCTexture::operator=(const mjCTexture& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCTexture_*>(this) = static_cast<const mjCTexture_&>(other);
}
PointToLocal();
return *this;
}
void mjCTexture::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
@@ -2897,20 +2942,14 @@ void mjCTexture::CopyFromSpec() {
cubefiles = (mjStringVec)&cubefiles_;
// clear precompiled asset. TODO: use asset cache
if (rgb) {
mju_free(rgb);
rgb = 0;
}
rgb.clear();
}
// free data storage allocated by lodepng
mjCTexture::~mjCTexture() {
if (rgb) {
mju_free(rgb);
rgb = 0;
}
rgb.clear();
}
@@ -2997,21 +3036,21 @@ void mjCTexture::Builtin2D(void) {
double pos = 2*sqrt(x*x+y*y) - 1;
// interpolate through sigmoid
interp(rgb + 3*(r*width+c), rgb2, rgb1, pos);
interp(rgb.data() + 3*(r*width+c), rgb2, rgb1, pos);
}
}
}
// checker
else if (builtin==mjBUILTIN_CHECKER) {
checker(rgb, RGB1, RGB2, width, height);
checker(rgb.data(), RGB1, RGB2, width, height);
}
// flat
else if (builtin==mjBUILTIN_FLAT) {
for (int r=0; r<height; r++) {
for (int c=0; c<width; c++) {
memcpy(rgb+3*(r*width+c), RGB1, 3);
memcpy(rgb.data()+3*(r*width+c), RGB1, 3);
}
}
}
@@ -3021,28 +3060,28 @@ void mjCTexture::Builtin2D(void) {
// edge
if (mark==mjMARK_EDGE) {
for (int r=0; r<height; r++) {
memcpy(rgb+3*(r*width+0), RGBm, 3);
memcpy(rgb+3*(r*width+width-1), RGBm, 3);
memcpy(rgb.data()+3*(r*width+0), RGBm, 3);
memcpy(rgb.data()+3*(r*width+width-1), RGBm, 3);
}
for (int c=0; c<width; c++) {
memcpy(rgb+3*(0*width+c), RGBm, 3);
memcpy(rgb+3*((height-1)*width+c), RGBm, 3);
memcpy(rgb.data()+3*(0*width+c), RGBm, 3);
memcpy(rgb.data()+3*((height-1)*width+c), RGBm, 3);
}
}
// cross
else if (mark==mjMARK_CROSS) {
for (int r=0; r<height; r++) {
memcpy(rgb+3*(r*width+width/2), RGBm, 3);
memcpy(rgb.data()+3*(r*width+width/2), RGBm, 3);
}
for (int c=0; c<width; c++) {
memcpy(rgb+3*(height/2*width+c), RGBm, 3);
memcpy(rgb.data()+3*(height/2*width+c), RGBm, 3);
}
}
// random dots
else if (mark==mjMARK_RANDOM && random>0) {
randomdot(rgb, markrgb, width, height, random);
randomdot(rgb.data(), markrgb, width, height, random);
}
}
@@ -3051,65 +3090,67 @@ void mjCTexture::Builtin2D(void) {
// make builtin: Cube
void mjCTexture::BuiltinCube(void) {
unsigned char RGB1[3], RGB2[3], RGBm[3], RGBi[3];
int w = width;
int ww = width*width;
// convert fixed colors
for (int j=0; j<3; j++) {
RGB1[j] = (mjtByte)(255*rgb1[j]);
RGB2[j] = (mjtByte)(255*rgb2[j]);
RGBm[j] = (mjtByte)(255*markrgb[j]);
for (int j = 0; j < 3; j++) {
RGB1[j] = (mjtByte)(255 * rgb1[j]);
RGB2[j] = (mjtByte)(255 * rgb2[j]);
RGBm[j] = (mjtByte)(255 * markrgb[j]);
}
//------------------ faces
// gradient
if (builtin==mjBUILTIN_GRADIENT) {
for (int r=0; r<width; r++) {
for (int c=0; c<width; c++) {
if (builtin == mjBUILTIN_GRADIENT) {
for (int r = 0; r < w; r++) {
for (int c = 0; c < w; c++) {
// compute normalized pixel coordinates
double x = 2*c/((double)(width-1)) - 1;
double y = 1 - 2*r/((double)(width-1));
double x = 2 * c / ((double)(w - 1)) - 1;
double y = 1 - 2 * r / ((double)(w - 1));
// compute normalized elevation for sides and up/down
double elside = asin(y/sqrt(1+x*x+y*y)) / (0.5*mjPI);
double elup = 1 - acos(1.0/sqrt(1+x*x+y*y)) / (0.5*mjPI);
double elside = asin(y / sqrt(1 + x * x + y * y)) / (0.5 * mjPI);
double elup = 1 - acos(1.0 / sqrt(1 + x * x + y * y)) / (0.5 * mjPI);
// set sides
interp(RGBi, rgb1, rgb2, elside);
memcpy(rgb+0*3*width*width+3*(r*width+c), RGBi, 3); // 0: right
memcpy(rgb+1*3*width*width+3*(r*width+c), RGBi, 3); // 1: left
memcpy(rgb+4*3*width*width+3*(r*width+c), RGBi, 3); // 4: front
memcpy(rgb+5*3*width*width+3*(r*width+c), RGBi, 3); // 5: back
memcpy(rgb.data() + 0 * 3 * ww + 3 * (r * w + c), RGBi, 3); // 0: right
memcpy(rgb.data() + 1 * 3 * ww + 3 * (r * w + c), RGBi, 3); // 1: left
memcpy(rgb.data() + 4 * 3 * ww + 3 * (r * w + c), RGBi, 3); // 4: front
memcpy(rgb.data() + 5 * 3 * ww + 3 * (r * w + c), RGBi, 3); // 5: back
// set up and down
interp(rgb+2*3*width*width+3*(r*width+c), rgb1, rgb2, elup); // 2: up
interp(rgb+3*3*width*width+3*(r*width+c), rgb1, rgb2, -elup); // 3: down
interp(rgb.data() + 2 * 3 * ww + 3 * (r * w + c), rgb1, rgb2, elup); // 2: up
interp(rgb.data() + 3 * 3 * ww + 3 * (r * w + c), rgb1, rgb2, -elup); // 3: down
}
}
}
// checker
else if (builtin==mjBUILTIN_CHECKER) {
checker(rgb+0*3*width*width, RGB1, RGB2, width, width);
checker(rgb+1*3*width*width, RGB1, RGB2, width, width);
checker(rgb+2*3*width*width, RGB1, RGB2, width, width);
checker(rgb+3*3*width*width, RGB1, RGB2, width, width);
checker(rgb+4*3*width*width, RGB2, RGB1, width, width);
checker(rgb+5*3*width*width, RGB2, RGB1, width, width);
else if (builtin == mjBUILTIN_CHECKER) {
checker(rgb.data() + 0 * 3 * ww, RGB1, RGB2, w, w);
checker(rgb.data() + 1 * 3 * ww, RGB1, RGB2, w, w);
checker(rgb.data() + 2 * 3 * ww, RGB1, RGB2, w, w);
checker(rgb.data() + 3 * 3 * ww, RGB1, RGB2, w, w);
checker(rgb.data() + 4 * 3 * ww, RGB2, RGB1, w, w);
checker(rgb.data() + 5 * 3 * ww, RGB2, RGB1, w, w);
}
// flat
else if (builtin==mjBUILTIN_FLAT) {
for (int r=0; r<width; r++) {
for (int c=0; c<width; c++) {
else if (builtin == mjBUILTIN_FLAT) {
for (int r = 0; r < w; r++) {
for (int c = 0; c < w; c++) {
// set sides and up
memcpy(rgb+0*3*width*width+3*(r*width+c), RGB1, 3);
memcpy(rgb+1*3*width*width+3*(r*width+c), RGB1, 3);
memcpy(rgb+2*3*width*width+3*(r*width+c), RGB1, 3);
memcpy(rgb+4*3*width*width+3*(r*width+c), RGB1, 3);
memcpy(rgb+5*3*width*width+3*(r*width+c), RGB1, 3);
memcpy(rgb.data() + 0 * 3 * ww + 3 * (r * w + c), RGB1, 3);
memcpy(rgb.data() + 1 * 3 * ww + 3 * (r * w + c), RGB1, 3);
memcpy(rgb.data() + 2 * 3 * ww + 3 * (r * w + c), RGB1, 3);
memcpy(rgb.data() + 4 * 3 * ww + 3 * (r * w + c), RGB1, 3);
memcpy(rgb.data() + 5 * 3 * ww + 3 * (r * w + c), RGB1, 3);
// set down
memcpy(rgb+3*3*width*width+3*(r*width+c), RGB2, 3);
memcpy(rgb.data() + 3 * 3 * ww + 3 * (r * w + c), RGB2, 3);
}
}
}
@@ -3117,39 +3158,37 @@ void mjCTexture::BuiltinCube(void) {
//------------------ marks
// edge
if (mark==mjMARK_EDGE) {
for (int j=0; j<6; j++) {
for (int r=0; r<width; r++) {
memcpy(rgb+j*3*width*width+3*(r*width+0), RGBm, 3);
memcpy(rgb+j*3*width*width+3*(r*width+width-1), RGBm, 3);
if (mark == mjMARK_EDGE) {
for (int j = 0; j < 6; j++) {
for (int r = 0; r < w; r++) {
memcpy(rgb.data() + j * 3 * ww + 3 * (r * w + 0), RGBm, 3);
memcpy(rgb.data() + j * 3 * ww + 3 * (r * w + w - 1), RGBm, 3);
}
for (int c=0; c<width; c++) {
memcpy(rgb+j*3*width*width+3*(0*width+c), RGBm, 3);
memcpy(rgb+j*3*width*width+3*((width-1)*width+c), RGBm, 3);
for (int c = 0; c < w; c++) {
memcpy(rgb.data() + j * 3 * ww + 3 * (0 * w + c), RGBm, 3);
memcpy(rgb.data() + j * 3 * ww + 3 * ((w - 1) * w + c), RGBm, 3);
}
}
}
// cross
else if (mark==mjMARK_CROSS) {
for (int j=0; j<6; j++) {
for (int r=0; r<width; r++) {
memcpy(rgb+j*3*width*width+3*(r*width+width/2), RGBm, 3);
else if (mark == mjMARK_CROSS) {
for (int j = 0; j < 6; j++) {
for (int r = 0; r < w; r++) {
memcpy(rgb.data() + j * 3 * ww + 3 * (r * w + w / 2), RGBm, 3);
}
for (int c=0; c<width; c++) {
memcpy(rgb+j*3*width*width+3*(width/2*width+c), RGBm, 3);
for (int c = 0; c < w; c++) {
memcpy(rgb.data() + j * 3 * ww + 3 * (w / 2 * w + c), RGBm, 3);
}
}
}
// random dots
else if (mark==mjMARK_RANDOM && random>0) {
randomdot(rgb, markrgb, width, height, random);
else if (mark == mjMARK_RANDOM && random > 0) {
randomdot(rgb.data(), markrgb, w, height, random);
}
}
// load PNG file
void mjCTexture::LoadPNG(mjResource* resource,
std::vector<unsigned char>& image,
@@ -3307,12 +3346,12 @@ void mjCTexture::Load2D(string filename, const mjVFS* vfs) {
height = h;
// allocate and copy data
rgb = (mjtByte*) mju_malloc(3*width*height);
if (!rgb) {
rgb.assign(3*width*height, 0);
if (rgb.empty()) {
throw mjCError(this, "Could not allocate memory for texture '%s' (id %d)",
(const char*)file_.c_str(), id);
}
memcpy(rgb, image.data(), 3*width*height);
memcpy(rgb.data(), image.data(), 3*width*height);
image.clear();
}
@@ -3348,8 +3387,8 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
}
// allocate data
rgb = (mjtByte*) mju_malloc(3*width*height);
if (!rgb) {
rgb.assign(3*width*height, 0);
if (rgb.empty()) {
throw mjCError(this,
"Could not allocate memory for texture '%s' (id %d)",
(const char*)file_.c_str(), id);
@@ -3357,7 +3396,7 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
// copy: repeated
if (gridsize[0]==1 && gridsize[1]==1) {
memcpy(rgb, image.data(), 3*width*width);
memcpy(rgb.data(), image.data(), 3*width*width);
}
// copy: grid
@@ -3391,7 +3430,7 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
int rstart = width*(k/gridsize[1]);
int cstart = width*(k%gridsize[1]);
for (int j=0; j<width; j++) {
memcpy(rgb+i*3*width*width+j*3*width, image.data()+(j+rstart)*3*w+3*cstart, 3*width);
memcpy(rgb.data()+i*3*width*width+j*3*width, image.data()+(j+rstart)*3*w+3*cstart, 3*width);
}
// mark as defined
@@ -3447,11 +3486,11 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
}
// first file: set size and allocate data
if (!rgb) {
if (rgb.empty()) {
width = w;
height = 6*width;
rgb = (mjtByte*) mju_malloc(3*width*height);
if (!rgb) {
rgb.assign(3*width*height, 0);
if (rgb.empty()) {
throw mjCError(this,
"Could not allocate memory for texture '%s' (id %d)",
(const char*)name.c_str(), id);
@@ -3466,7 +3505,7 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
}
// copy data
memcpy(rgb+i*3*width*width, image.data(), 3*width*width);
memcpy(rgb.data()+i*3*width*width, image.data(), 3*width*width);
image.clear();
// mark as defined
@@ -3509,8 +3548,8 @@ void mjCTexture::Compile(const mjVFS* vfs) {
}
// allocate data
rgb = (mjtByte*) mju_malloc(3*width*height);
if (!rgb) {
rgb.assign(3*width*height, 0);
if (rgb.empty()) {
throw mjCError(this,
"Could not allocate memory for texture '%s' (id %d)",
(const char*)name.c_str(), id);
@@ -3570,7 +3609,7 @@ void mjCTexture::Compile(const mjVFS* vfs) {
}
// make sure someone allocated data; SHOULD NOT OCCUR
if (!rgb) {
if (rgb.empty()) {
throw mjCError(this,
"texture '%s' (id %d) was not specified", (const char*)name.c_str(), id);
}
@@ -3877,6 +3916,24 @@ mjCBodyPair::mjCBodyPair(mjCModel* _model) {
mjCBodyPair::mjCBodyPair(const mjCBodyPair& other) {
*this = other;
}
mjCBodyPair& mjCBodyPair::operator=(const mjCBodyPair& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCBodyPair_*>(this) = static_cast<const mjCBodyPair_&>(other);
*static_cast<mjmExclude*>(this) = static_cast<const mjmExclude&>(other);
}
PointToLocal();
return *this;
}
void mjCBodyPair::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
@@ -4114,8 +4171,12 @@ mjCTendon::mjCTendon(const mjCTendon& other) {
mjCTendon& mjCTendon::operator=(const mjCTendon& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCTendon_*>(this) = static_cast<const mjCTendon_&>(other);
*static_cast<mjmTendon*>(this) = static_cast<const mjmTendon&>(other);
*static_cast<mjmTendon*>(this) = static_cast<const mjmTendon&>(other);
for (int i=0; i<other.path.size(); i++) {
path.push_back(new mjCWrap(*other.path[i]));
path.back()->tendon = this;
}
}
PointToLocal();
return *this;
@@ -4166,6 +4227,15 @@ mjCTendon::~mjCTendon() {
void mjCTendon::SetModel(mjCModel* _model) {
model = _model;
for (int i=0; i<path.size(); i++) {
path[i]->model = _model;
}
}
// add site as wrap object
void mjCTendon::WrapSite(string name, std::string_view info) {
// create wrap object
@@ -4395,6 +4465,25 @@ mjCWrap::mjCWrap(mjCModel* _model, mjCTendon* _tendon) {
mjCWrap::mjCWrap(const mjCWrap& other) {
*this = other;
}
mjCWrap& mjCWrap::operator=(const mjCWrap& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCWrap_*>(this) = static_cast<const mjCWrap_&>(other);
*static_cast<mjmWrap*>(this) = static_cast<const mjmWrap&>(other);
obj = nullptr;
}
PointToLocal();
return *this;
}
void mjCWrap::PointToLocal() {
spec.element = (mjElement)this;
spec.info = (mjString)&info;
@@ -4822,12 +4911,30 @@ mjCSensor::mjCSensor(mjCModel* _model) {
CopyFromSpec();
// point to local
MakePointerLocal();
PointToLocal();
}
void mjCSensor::MakePointerLocal() {
mjCSensor::mjCSensor(const mjCSensor& other) {
*this = other;
}
mjCSensor& mjCSensor::operator=(const mjCSensor& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCSensor_*>(this) = static_cast<const mjCSensor_&>(other);
*static_cast<mjmSensor*>(this) = static_cast<const mjmSensor&>(other);
}
PointToLocal();
return *this;
}
void mjCSensor::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
@@ -5271,6 +5378,24 @@ mjCNumeric::mjCNumeric(mjCModel* _model) {
mjCNumeric::mjCNumeric(const mjCNumeric& other) {
*this = other;
}
mjCNumeric& mjCNumeric::operator=(const mjCNumeric& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCNumeric_*>(this) = static_cast<const mjCNumeric_&>(other);
*static_cast<mjmNumeric*>(this) = static_cast<const mjmNumeric&>(other);
}
PointToLocal();
return *this;
}
void mjCNumeric::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
@@ -5341,6 +5466,24 @@ mjCText::mjCText(mjCModel* _model) {
mjCText::mjCText(const mjCText& other) {
*this = other;
}
mjCText& mjCText::operator=(const mjCText& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCText_*>(this) = static_cast<const mjCText_&>(other);
*static_cast<mjmText*>(this) = static_cast<const mjmText&>(other);
}
PointToLocal();
return *this;
}
void mjCText::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
@@ -5402,6 +5545,24 @@ mjCTuple::mjCTuple(mjCModel* _model) {
mjCTuple::mjCTuple(const mjCTuple& other) {
*this = other;
}
mjCTuple& mjCTuple::operator=(const mjCTuple& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCTuple_*>(this) = static_cast<const mjCTuple_&>(other);
*static_cast<mjmTuple*>(this) = static_cast<const mjmTuple&>(other);
}
PointToLocal();
return *this;
}
void mjCTuple::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
@@ -5502,6 +5663,24 @@ mjCKey::mjCKey(mjCModel* _model) {
mjCKey::mjCKey(const mjCKey& other) {
*this = other;
}
mjCKey& mjCKey::operator=(const mjCKey& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCKey_*>(this) = static_cast<const mjCKey_&>(other);
*static_cast<mjmKey*>(this) = static_cast<const mjmKey&>(other);
}
PointToLocal();
return *this;
}
void mjCKey::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
@@ -5653,6 +5832,23 @@ mjCPlugin::mjCPlugin(mjCModel* _model) {
mjCPlugin::mjCPlugin(const mjCPlugin& other) {
*this = other;
}
mjCPlugin& mjCPlugin::operator=(const mjCPlugin& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCPlugin_*>(this) = static_cast<const mjCPlugin_&>(other);
parent = this;
}
return *this;
}
// compiler
void mjCPlugin::Compile(void) {
const mjpPlugin* plugin = mjp_getPluginAtSlot(spec.plugin_slot);
src/user/user_objects.h
+14 -12
View File
@@ -177,7 +177,6 @@ class mjCBase_ {
std::string classname; // defaults class name
int id; // object id
std::string info; // error message info set by the user
mjCModel* model; // pointer to model that created object
};
class mjCBase : public mjCBase_ {
@@ -202,6 +201,7 @@ class mjCBase : public mjCBase_ {
mjCDef* def; // defaults class used to init this object
mjCFrame* frame; // pointer to frame transformation
mjCModel* model; // pointer to model that created object
protected:
mjCBase(); // constructor
@@ -296,10 +296,10 @@ class mjCBody : public mjCBody_, private mjmBody {
const std::vector<double>& get_userdata() { return userdata_; }
private:
mjCBody(mjCModel*); // constructor
mjCBody(const mjCBody& other); // copy constructor
mjCBody& operator=(const mjCBody& other); // copy assignment
~mjCBody(); // destructor
mjCBody(mjCModel*); // constructor
mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor
mjCBody& operator=(const mjCBody& other); // copy assignment
~mjCBody(); // destructor
void Compile(void); // compiler
void GeomFrame(void); // get inertial info from geoms
@@ -755,6 +755,7 @@ class mjCMesh_ : public mjCBase {
};
class mjCMesh: public mjCMesh_, private mjmMesh {
friend class mjCModel;
friend class mjCFlexcomp;
friend class mjXWriter;
public:
@@ -932,6 +933,8 @@ class mjCSkin: public mjCSkin_, private mjmSkin {
class mjCHField_ : public mjCBase {
protected:
std::vector<float> data; // elevation data, row-major format
std::string file_;
std::string content_type_;
std::vector<float> userdata_;
@@ -964,7 +967,6 @@ class mjCHField : public mjCHField_, private mjmHField {
mjCHField& operator=(const mjCHField& other); // copy assignment
~mjCHField(); // destructor
float* data; // elevation data, row-major format
void Compile(const mjVFS* vfs); // compiler
void LoadCustom(mjResource* resource); // load from custom format
@@ -978,6 +980,8 @@ class mjCHField : public mjCHField_, private mjmHField {
class mjCTexture_ : public mjCBase {
protected:
std::vector<mjtByte> rgb; // rgb data
std::string file_;
std::string content_type_;
std::vector<std::string> cubefiles_;
@@ -1029,8 +1033,6 @@ class mjCTexture : public mjCTexture_, private mjmTexture {
void LoadCustom(mjResource* resource,
std::vector<unsigned char>& image,
unsigned int& w, unsigned int& h);
mjtByte* rgb; // rgb data
};
@@ -1247,6 +1249,7 @@ class mjCTendon : public mjCTendon_, private mjmTendon {
void CopyFromSpec();
void PointToLocal();
void SetModel(mjCModel* _model);
bool is_limited() const;
@@ -1315,10 +1318,10 @@ class mjCPlugin : public mjCPlugin_ {
public:
mjmPlugin spec;
mjCBase* parent; // parent object (only used when generating error message)
mjCPlugin(const mjCPlugin& other); // copy constructor
private:
mjCPlugin(mjCModel*); // constructor
mjCPlugin(const mjCPlugin& other); // copy constructor
mjCPlugin& operator=(const mjCPlugin& other); // copy assignment
void Compile(void); // compiler
@@ -1385,6 +1388,7 @@ class mjCActuator : public mjCActuator_, private mjmActuator {
class mjCSensor_ : public mjCBase {
protected:
int refid; // id of reference frame
mjCBase* obj; // sensorized object
// variable-size data
std::string plugin_name;
@@ -1420,9 +1424,7 @@ class mjCSensor : public mjCSensor_, private mjmSensor {
void Compile(void); // compiler
void CopyFromSpec();
void MakePointerLocal();
mjCBase* obj; // sensorized object
void PointToLocal();
};
test/user/user_api_test.cc
+41 -16
View File
@@ -65,6 +65,9 @@ TEST_F(MujocoTest, ReadWriteData) {
// ------------------- test recompilation multiple files ----------------------
TEST_F(PluginTest, RecompileCompare) {
mjtNum tol = 0;
std::string field = "";
// full precision float printing
FullFloatPrecision increase_precision;
@@ -85,33 +88,55 @@ TEST_F(PluginTest, RecompileCompare) {
continue;
}
// load model
std::array<char, 1000> error;
mjSpec* spec =
mjParseXML(xml.c_str(), nullptr, error.data(), error.size());
// load spec
std::array<char, 1000> err;
mjSpec* s = mjParseXML(xml.c_str(), nullptr, err.data(), err.size());
// compile twice
mjModel* m_old = mjm_compile(spec, nullptr);
mjModel* m_new = mjm_compile(spec, nullptr);
// copy spec
mjSpec* s_copy = mjm_copySpec(s);
// compile twice and compare
mjModel* m_old = mjm_compile(s, nullptr);
mjModel* m_new = mjm_compile(s, nullptr);
mjModel* m_copy = mjm_compile(s_copy, nullptr);
ASSERT_THAT(m_old, NotNull())
<< "Failed to compile " << xml << ": " << error.data();
<< "Failed to compile " << xml << ": " << err.data();
ASSERT_THAT(m_new, NotNull())
<< "Failed to recompile " << xml << ": " << error.data();
<< "Failed to recompile " << xml << ": " << err.data();
ASSERT_THAT(m_copy, NotNull())
<< "Failed to compile " << xml << ": " << err.data();
// compare and delete
std::string field = "";
mjtNum result = CompareModel(m_old, m_new, field);
mjtNum tol = 0;
EXPECT_LE(result, tol)
<< "Loaded and saved models are different!\n"
EXPECT_LE(CompareModel(m_old, m_new, field), tol)
<< "Compiled and recompiled models are different!\n"
<< "Affected file " << p.path().string() << '\n'
<< "Different field: " << field << '\n';
EXPECT_LE(CompareModel(m_old, m_copy, field), tol)
<< "Original and copied models are different!\n"
<< "Affected file " << p.path().string() << '\n'
<< "Different field: " << field << '\n';
// copy to a new spec, compile and compare
mjSpec* s_copy2 = mjm_copySpec(s);
mjModel* m_copy2 = mjm_compile(s_copy2, nullptr);
ASSERT_THAT(m_copy2, NotNull())
<< "Failed to compile " << xml << ": " << err.data();
EXPECT_LE(CompareModel(m_old, m_copy2, field), tol)
<< "Original and re-copied models are different!\n"
<< "Affected file " << p.path().string() << '\n'
<< "Different field: " << field << '\n';
// delete models
mjm_deleteSpec(spec);
mjm_deleteSpec(s);
mjm_deleteSpec(s_copy);
mjm_deleteSpec(s_copy2);
mj_deleteModel(m_old);
mj_deleteModel(m_new);
mj_deleteModel(m_copy);
mj_deleteModel(m_copy2);
}
}
}