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

Add mjStatistic to mjmModel.

Browse Source 
PiperOrigin-RevId: 607657424
Change-Id: I17f6ef37570651e002258ee2a509fc52d213c0f5
This commit is contained in:
Alessio Quaglino
2024-02-16 05:16:47 -08:00
committed by Copybara-Service
parent f1d4f8f0a1
commit d7c47de353
6 changed files with 55 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.h
+4
View File
@@ -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);
src/user/user_init.cc
+14
View File
@@ -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));
src/user/user_model.cc
+19 -18
View File
@@ -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<mjmModel*>(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
src/user/user_model.h
+3 -21
View File
@@ -25,6 +25,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include "user/user_api.h"
#include "user/user_objects.h"
typedef enum _mjtInertiaFromGeom {
@@ -46,31 +47,19 @@ typedef std::array<mjKeyMap, mjNOBJECT> 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
src/xml/xml_native_reader.cc
+9 -8
View File
@@ -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) {
src/xml/xml_native_writer.cc
+6 -5
View File
@@ -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);