From d7c47de3537769239a51ea067d3cd87c004a0686 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 16 Feb 2024 05:16:47 -0800 Subject: [PATCH] Add mjStatistic to mjmModel. PiperOrigin-RevId: 607657424 Change-Id: I17f6ef37570651e002258ee2a509fc52d213c0f5 --- src/user/user_api.h | 4 ++++ src/user/user_init.cc | 14 ++++++++++++++ src/user/user_model.cc | 37 ++++++++++++++++++------------------ src/user/user_model.h | 24 +++-------------------- src/xml/xml_native_reader.cc | 17 +++++++++-------- src/xml/xml_native_writer.cc | 11 ++++++----- 6 files changed, 55 insertions(+), 52 deletions(-) diff --git a/src/user/user_api.h b/src/user/user_api.h index a446d68f..410242e5 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -73,6 +73,7 @@ typedef enum _mjtLimited { // type of limit specification typedef struct _mjmModel { // model specification mjElement element; // internal, do not modify + mjStatistic stat; // statistics override (if defined) } mjmModel; typedef struct _mjmOrientation { // alternative orientation specifiers @@ -825,6 +826,9 @@ MJAPI const char* mjm_setFullInertia(mjmBody* body, double quat[4], double inert //---------------------------------- Initialization functions -------------------------------------- +// Default model attributes. +MJAPI void mjm_defaultModel(mjmModel& model); + // Default body attributes. MJAPI void mjm_defaultBody(mjmBody& body); diff --git a/src/user/user_init.cc b/src/user/user_init.cc index 0b91bfac..d58c84cb 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -21,6 +21,20 @@ +// default model attributes +void mjm_defaultModel(mjmModel& model) { + memset(&model, 0, sizeof(mjmModel)); + + // default statistics + model.stat.meaninertia = mjNAN; + model.stat.meanmass = mjNAN; + model.stat.meansize = mjNAN; + model.stat.extent = mjNAN; + model.stat.center[0] = mjNAN; +} + + + // default body attributes void mjm_defaultBody(mjmBody& body) { memset(&body, 0, sizeof(mjmBody)); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 49b31542..782d0a8f 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -83,6 +83,7 @@ static void copyvec(T1* dest, T2* src, int n) { // constructor mjCModel::mjCModel() { + mjm_defaultModel(spec); comment.clear(); modelfiledir.clear(); @@ -110,14 +111,6 @@ mjCModel::mjCModel() { exactmeshinertia = false; mj_defaultLROpt(&LRopt); - //------------------------ statistics override - meaninertia = mjNAN; - meanmass = mjNAN; - meansize = mjNAN; - extent = mjNAN; - center[0] = mjNAN; - center[1] = center[2] = 0; - //------------------------ auto-computed statistics #ifndef MEMORY_SANITIZER // initializing as best practice, but want MSAN to catch unintialized use @@ -223,6 +216,12 @@ mjCModel::mjCModel() { +void mjCModel::CopyFromSpec() { + *static_cast(this) = spec; +} + + + // destructor mjCModel::~mjCModel() { // delete kinematic tree and all objects allocated in it @@ -2748,6 +2747,8 @@ static void warninghandler(const char* msg) { // compiler mjModel* mjCModel::Compile(const mjVFS* vfs) { + CopyFromSpec(); + // The volatile keyword is necessary to prevent a possible memory leak due to // an interaction between longjmp and compiler optimization. Specifically, at // the point where the setjmp takes places, these pointers have never been @@ -3208,11 +3209,11 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { copyvec(center_auto, m->stat.center, 3); // override model statistics if defined by user - if (mjuu_defined(extent)) m->stat.extent = (mjtNum)extent; - if (mjuu_defined(meaninertia)) m->stat.meaninertia = (mjtNum)meaninertia; - if (mjuu_defined(meanmass)) m->stat.meanmass = (mjtNum)meanmass; - if (mjuu_defined(meansize)) m->stat.meansize = (mjtNum)meansize; - if (mjuu_defined(center[0])) copyvec(m->stat.center, center, 3); + if (mjuu_defined(stat.extent)) m->stat.extent = (mjtNum)stat.extent; + if (mjuu_defined(stat.meaninertia)) m->stat.meaninertia = (mjtNum)stat.meaninertia; + if (mjuu_defined(stat.meanmass)) m->stat.meanmass = (mjtNum)stat.meanmass; + if (mjuu_defined(stat.meansize)) m->stat.meansize = (mjtNum)stat.meansize; + if (mjuu_defined(stat.center[0])) copyvec(m->stat.center, stat.center, 3); // assert that model has valid references const char* validationerr = mj_validateReferences(m); @@ -3284,14 +3285,14 @@ bool mjCModel::CopyBack(const mjModel* m) { visual = m->vis; // runtime-modifiable members of mjStatistic, if different from computed values - if (m->stat.meaninertia != meaninertia_auto) meaninertia = m->stat.meaninertia; - if (m->stat.meanmass != meanmass_auto) meanmass = m->stat.meanmass; - if (m->stat.meansize != meansize_auto) meansize = m->stat.meansize; - if (m->stat.extent != extent_auto) extent = m->stat.extent; + if (m->stat.meaninertia != meaninertia_auto) stat.meaninertia = m->stat.meaninertia; + if (m->stat.meanmass != meanmass_auto) stat.meanmass = m->stat.meanmass; + if (m->stat.meansize != meansize_auto) stat.meansize = m->stat.meansize; + if (m->stat.extent != extent_auto) stat.extent = m->stat.extent; if (m->stat.center[0] != center_auto[0] || m->stat.center[1] != center_auto[1] || m->stat.center[2] != center_auto[2]) { - mju_copy3(center, m->stat.center); + mju_copy3(stat.center, m->stat.center); } // qpos0, qpos_spring diff --git a/src/user/user_model.h b/src/user/user_model.h index 15d5ae31..ff3ec5fa 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -25,6 +25,7 @@ #include #include #include +#include "user/user_api.h" #include "user/user_objects.h" typedef enum _mjtInertiaFromGeom { @@ -46,31 +47,19 @@ typedef std::array mjListKeyMap; // 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 { +class mjCModel : private mjmModel { friend class mjCBody; - friend class mjCJoint; friend class mjCGeom; friend class mjCFlex; - friend class mjCMesh; - friend class mjCSkin; - friend class mjCHField; - friend class mjCPair; - friend class mjCBodyPair; - friend class mjCSite; friend class mjCEquality; friend class mjCTendon; - friend class mjCWrap; - friend class mjCActuator; - friend class mjCSensor; - friend class mjCNumeric; - friend class mjCTuple; - friend class mjCKey; friend class mjXReader; friend class mjXWriter; public: mjCModel(); // constructor ~mjCModel(); // destructor + void CopyFromSpec(); // copy spec to private attributes mjmModel spec; @@ -154,13 +143,6 @@ class mjCModel { bool exactmeshinertia; // if false, use old formula mjLROpt LRopt; // options for lengthrange computation - //------------------------ statistics override (if defined) - double meaninertia; // mean diagonal inertia - double meanmass; // mean body mass - double meansize; // mean body size - double extent; // spatial extent - double center[3]; // center of model - //------------------------ engine data std::string modelname; // model name mjOption option; // options diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index bb5f82d7..61e96b7c 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1253,14 +1253,15 @@ void mjXReader::Statistic(XMLElement* section) { string text; // read statistics - ReadAttr(section, "meaninertia", 1, &model->meaninertia, text); - ReadAttr(section, "meanmass", 1, &model->meanmass, text); - ReadAttr(section, "meansize", 1, &model->meansize, text); - ReadAttr(section, "extent", 1, &model->extent, text); - if (mjuu_defined(model->extent) && model->extent<=0) { + mjmModel* pmodel = &model->spec; + ReadAttr(section, "meaninertia", 1, &pmodel->stat.meaninertia, text); + ReadAttr(section, "meanmass", 1, &pmodel->stat.meanmass, text); + ReadAttr(section, "meansize", 1, &pmodel->stat.meansize, text); + ReadAttr(section, "extent", 1, &pmodel->stat.extent, text); + if (mjuu_defined(pmodel->stat.extent) && pmodel->stat.extent<=0) { throw mjXError(section, "extent must be strictly positive"); } - ReadAttr(section, "center", 3, model->center, text); + ReadAttr(section, "center", 3, pmodel->stat.center, text); } @@ -2482,7 +2483,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjmBody* pbody, mjmDefault* def) // make composite char error[200]; - bool res = comp.Make((mjCModel*)mjm_getModel(pbody), pbody, error, 200); + bool res = comp.Make(model, pbody, error, 200); // throw error if (!res) { @@ -2617,7 +2618,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjmBody* pbody) { // make flexcomp char error[200]; - bool res = fcomp.Make((mjCModel*)mjm_getModel(pbody), pbody, error, 200); + bool res = fcomp.Make(model, pbody, error, 200); // throw error if (!res) { diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index fa813b48..3eae2fe1 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -975,12 +975,13 @@ void mjXWriter::Size(XMLElement* root) { // statistic section void mjXWriter::Statistic(XMLElement* root) { XMLElement* section = InsertEnd(root, "statistic"); + mjStatistic* s = &model->stat; - if (mjuu_defined(model->meaninertia)) WriteAttr(section, "meaninertia", 1, &model->meaninertia); - if (mjuu_defined(model->meanmass)) WriteAttr(section, "meanmass", 1, &model->meanmass); - if (mjuu_defined(model->meansize)) WriteAttr(section, "meansize", 1, &model->meansize); - if (mjuu_defined(model->extent)) WriteAttr(section, "extent", 1, &model->extent); - if (mjuu_defined(model->center[0])) WriteAttr(section, "center", 3, model->center); + if (mjuu_defined(s->meaninertia)) WriteAttr(section, "meaninertia", 1, &s->meaninertia); + if (mjuu_defined(s->meanmass)) WriteAttr(section, "meanmass", 1, &s->meanmass); + if (mjuu_defined(s->meansize)) WriteAttr(section, "meansize", 1, &s->meansize); + if (mjuu_defined(s->extent)) WriteAttr(section, "extent", 1, &s->extent); + if (mjuu_defined(s->center[0])) WriteAttr(section, "center", 3, s->center); // remove entire section if no attributes if (!section->FirstAttribute()) root->DeleteChild(section);