Add boolean flags for gravity compensation and surface velocity in mjModel.
These fields (`flg_gravcomp` and `flg_surfacevel`) replace the fast-path checks originally guarded by `ngravcomp` and (recently) `nsurfacevel`. Since the engine uses these integers only as flags (zero vs non-zero), migrating them to actual booleans makes them writeable from the Python bindings at runtime without violating size/dimension constraints. The legacy integer field `ngravcomp` is marked as deprecated and will be removed in a future release. PiperOrigin-RevId: 949779204 Change-Id: Ifab1f026063a4239302e6ad689663b611b59dda8
This commit is contained in:
committed by
Copybara-Service
parent
ebd4abae03
commit
a04b0c5b97
@@ -3130,7 +3130,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
|
||||
// add relative surface velocity of contacting geoms to contact rows of efc_vel
|
||||
static void mj_addSurfaceVel(const mjModel* m, mjData* d) {
|
||||
// no surface velocity on any geom: quick return
|
||||
if (!m->nsurfacevel) {
|
||||
if (!m->flg_surfacevel) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -794,7 +794,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||
d->moment_rownnz, d->moment_rowadr, d->moment_colind);
|
||||
|
||||
// actuator-level gravity compensation
|
||||
if (m->ngravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
|
||||
if (m->flg_gravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
|
||||
// number of dofs for each joint type: {mjJNT_FREE, mjJNT_BALL, mjJNT_SLIDE, mjJNT_HINGE}
|
||||
static const int jnt_dofnum[4] = {6, 3, 1, 1};
|
||||
int njnt = m->njnt;
|
||||
|
||||
+10
-4
@@ -250,7 +250,7 @@ void mj_makeModel(mjModel** dest,
|
||||
// CHECK SIZE PARAMETERS
|
||||
{
|
||||
// dummy variables for MJMODEL_SIZES set after mjModel construction
|
||||
int nnames_map = 0, nJmom = 0, ngravcomp = 0, nsurfacevel = 0, nemax = 0, njmax = 0, nconmax=0;
|
||||
int nnames_map = 0, nJmom = 0, ngravcomp = 0, nemax = 0, njmax = 0, nconmax=0;
|
||||
int npolygonmax = 0, nmeshdegmax = 0;
|
||||
int nuserdata=0, nsensordata=0, npluginstate=0, nhistory=0, narena=0, nbuffer=0;
|
||||
|
||||
@@ -270,7 +270,7 @@ void mj_makeModel(mjModel** dest,
|
||||
#undef X
|
||||
|
||||
// suppress unused variable warnings
|
||||
(void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nsurfacevel; (void)nemax; (void)njmax; (void)nconmax;
|
||||
(void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax;
|
||||
(void)npolygonmax; (void)nmeshdegmax;
|
||||
(void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena;
|
||||
(void)nbuffer;
|
||||
@@ -543,6 +543,8 @@ void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buff
|
||||
bufwrite((void*)&m->opt, sizeof(mjOption), buffer_sz, buffer, &ptrbuf);
|
||||
bufwrite((void*)&m->vis, sizeof(mjVisual), buffer_sz, buffer, &ptrbuf);
|
||||
bufwrite((void*)&m->stat, sizeof(mjStatistic), buffer_sz, buffer, &ptrbuf);
|
||||
bufwrite(&m->flg_gravcomp, sizeof(mjtBool), buffer_sz, buffer, &ptrbuf);
|
||||
bufwrite(&m->flg_surfacevel, sizeof(mjtBool), buffer_sz, buffer, &ptrbuf);
|
||||
{
|
||||
MJMODEL_POINTERS_PREAMBLE(m)
|
||||
#define X(type, name, nr, nc) \
|
||||
@@ -644,13 +646,16 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
|
||||
}
|
||||
|
||||
// read options and buffer
|
||||
if (ptrbuf + sizeof(mjOption) + sizeof(mjVisual) + sizeof(mjStatistic) > buffer_sz) {
|
||||
if (ptrbuf + sizeof(mjOption) + sizeof(mjVisual) + sizeof(mjStatistic) +
|
||||
sizeof(mjtBool) * 2 > buffer_sz) {
|
||||
mju_warning("Truncated model file - ran out of data while reading structs");
|
||||
return NULL;
|
||||
}
|
||||
bufread((void*)&m->opt, sizeof(mjOption), buffer_sz, buffer, &ptrbuf);
|
||||
bufread((void*)&m->vis, sizeof(mjVisual), buffer_sz, buffer, &ptrbuf);
|
||||
bufread((void*)&m->stat, sizeof(mjStatistic), buffer_sz, buffer, &ptrbuf);
|
||||
bufread(&m->flg_gravcomp, sizeof(mjtBool), buffer_sz, buffer, &ptrbuf);
|
||||
bufread(&m->flg_surfacevel, sizeof(mjtBool), buffer_sz, buffer, &ptrbuf);
|
||||
{
|
||||
MJMODEL_POINTERS_PREAMBLE(m)
|
||||
#define X(type, name, nr, nc) \
|
||||
@@ -700,7 +705,8 @@ mjtSize mj_sizeModel(const mjModel* m) {
|
||||
+ sizeof(mjtSize)*getnsize()
|
||||
+ sizeof(mjOption)
|
||||
+ sizeof(mjVisual)
|
||||
+ sizeof(mjStatistic));
|
||||
+ sizeof(mjStatistic)
|
||||
+ sizeof(mjtBool)*2);
|
||||
|
||||
MJMODEL_POINTERS_PREAMBLE(m)
|
||||
#define X(type, name, nr, nc) \
|
||||
|
||||
@@ -821,7 +821,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
||||
|
||||
// body-level gravity compensation, return 1 if any, 0 otherwise
|
||||
static int mj_gravcomp(const mjModel* m, mjData* d) {
|
||||
if (!m->ngravcomp || mjDISABLED(mjDSBL_GRAVITY) || mju_norm3(m->opt.gravity) == 0) {
|
||||
if (!m->flg_gravcomp || mjDISABLED(mjDSBL_GRAVITY) || mju_norm3(m->opt.gravity) == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -569,6 +569,12 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
|
||||
#undef X
|
||||
fprintf(fp, "\n");
|
||||
|
||||
// flags
|
||||
fprintf(fp, "FLAG\n");
|
||||
printInt(fp, " flg_gravcomp", m->flg_gravcomp);
|
||||
printInt(fp, " flg_surfacevel", m->flg_surfacevel);
|
||||
fprintf(fp, "\n");
|
||||
|
||||
// options
|
||||
fprintf(fp, "OPTION\n");
|
||||
#define X(type, name, sz) \
|
||||
|
||||
@@ -210,16 +210,18 @@ static void setFixed(mjModel* m, mjData* d) {
|
||||
ngravcomp += (m->body_gravcomp[i] > 0);
|
||||
}
|
||||
m->ngravcomp = ngravcomp;
|
||||
m->flg_gravcomp = (ngravcomp > 0);
|
||||
|
||||
// compute nsurfacevel: number of geoms with nonzero surfacevel
|
||||
int nsurfacevel = 0;
|
||||
// compute flg_surfacevel: whether any geom has nonzero surfacevel
|
||||
mjtBool flg_surfacevel = 0;
|
||||
for (int i=0; i < m->ngeom; i++) {
|
||||
const mjtNum* sv = m->geom_surfacevel + 6*i;
|
||||
if (sv[0] || sv[1] || sv[2] || sv[3] || sv[4] || sv[5]) {
|
||||
nsurfacevel++;
|
||||
flg_surfacevel = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
m->nsurfacevel = nsurfacevel;
|
||||
m->flg_surfacevel = flg_surfacevel;
|
||||
|
||||
// set jnt_actuatorid and tendon_actuatorid
|
||||
mju_fillInt(m->jnt_actuatorid, -1, m->njnt);
|
||||
|
||||
Reference in New Issue
Block a user