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
+6
-1
@@ -72,7 +72,12 @@ General
|
||||
integrator and flex stiffness present; Newton and PGS are unaffected. Bending-only models pay zero per-step
|
||||
factorization cost (the factor is precomputed in :ref:`mj_setConst<mj_setConst>`). Inverse dynamics
|
||||
(:ref:`mj_inverse<mj_inverse>`) is now discrete-consistent with forward dynamics for gated models.
|
||||
- The ``mjz`` decoder now searches for ``model.xml`` at the root of the archive as a fallback if the archive-named XML is not found.
|
||||
- The ``mjz`` decoder now searches for ``model.xml`` at the root of the archive as a fallback if the archive-named XML
|
||||
is not found.
|
||||
- Added ``flg_gravcomp`` and ``flg_surfacevel`` boolean flags to ``mjModel``. These flags replace the fast-path checks
|
||||
as originally guarded by ``ngravcomp``. Since the engine uses these integers as flags (zero vs. non-zero), the new
|
||||
flags are honest boolean properties, writeable from the Python bindings at runtime. The field ``ngravcomp`` is
|
||||
deprecated and will be removed in a future release.
|
||||
|
||||
.. admonition:: Breaking API changes
|
||||
:class: attention
|
||||
|
||||
@@ -661,7 +661,6 @@ typedef struct mjModel_ {
|
||||
mjtSize nnames_map; // number of slots in the names hash map
|
||||
mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix
|
||||
mjtSize ngravcomp; // number of bodies with nonzero gravcomp
|
||||
mjtSize nsurfacevel; // number of geoms with nonzero surfacevel
|
||||
mjtSize nemax; // number of potential equality-constraint rows
|
||||
mjtSize njmax; // number of available rows in constraint Jacobian (legacy)
|
||||
mjtSize nconmax; // number of potential contacts in contact list (legacy)
|
||||
@@ -676,6 +675,11 @@ typedef struct mjModel_ {
|
||||
mjtSize narena; // number of bytes in the mjData arena (inclusive of stack)
|
||||
mjtSize nbuffer; // number of bytes in buffer
|
||||
|
||||
// ------------------------------- flags
|
||||
|
||||
mjtBool flg_gravcomp; // whether any body has nonzero gravcomp
|
||||
mjtBool flg_surfacevel; // whether any geom has nonzero surfacevel
|
||||
|
||||
// ------------------------------- options and statistics
|
||||
|
||||
mjOption opt; // physics options
|
||||
|
||||
@@ -715,8 +715,9 @@ Exceptions to the general rule that **real-valued** types **are safe to change**
|
||||
- Unsafe for static bodies, invalidates the midphase collision structures (BVH).
|
||||
* - ``body_gravcomp``
|
||||
- Safe.
|
||||
- If the number of bodies with gravity compensation is changed from zero to non-zero,
|
||||
:ref:`mj_setConst` must be called.
|
||||
- If passing from a state where all bodies have zero gravity compensation to a state where some bodies have
|
||||
non-zero gravity compensation (or vice-versa), the ``flg_gravcomp`` flag in :ref:`mjModel` must be updated.
|
||||
This can be done directly or by calling :ref:`mj_setConst`.
|
||||
* - ``dof_armature``
|
||||
- Safe with :ref:`mj_setConst`.
|
||||
-
|
||||
@@ -725,8 +726,9 @@ Exceptions to the general rule that **real-valued** types **are safe to change**
|
||||
-
|
||||
* - ``geom_surfacevel``
|
||||
- Safe.
|
||||
- If the number of geoms with nonzero surface velocity is changed from zero to non-zero (or vice versa),
|
||||
:ref:`mj_setConst` must be called.
|
||||
- If passing from a state where all geoms have zero surface velocity to a state where some geoms have
|
||||
non-zero surface velocity (or vice-versa), the ``flg_surfacevel`` flag in :ref:`mjModel` must be updated.
|
||||
This can be done directly or by calling :ref:`mj_setConst`.
|
||||
* - ``{site,cam,light}_`` |br| ``{pos,quat}``
|
||||
- Mostly safe.
|
||||
- For cameras and lights with tracking or targeting, :ref:`mj_setConst` is required.
|
||||
|
||||
@@ -332,7 +332,6 @@ typedef struct mjModel_ {
|
||||
mjtSize nnames_map; // number of slots in the names hash map
|
||||
mjtSize nJmom; // number of non-zeros in sparse actuator_moment matrix
|
||||
mjtSize ngravcomp; // number of bodies with nonzero gravcomp
|
||||
mjtSize nsurfacevel; // number of geoms with nonzero surfacevel
|
||||
mjtSize nemax; // number of potential equality-constraint rows
|
||||
mjtSize njmax; // number of available rows in constraint Jacobian (legacy)
|
||||
mjtSize nconmax; // number of potential contacts in contact list (legacy)
|
||||
@@ -347,6 +346,11 @@ typedef struct mjModel_ {
|
||||
mjtSize narena; // number of bytes in the mjData arena (inclusive of stack)
|
||||
mjtSize nbuffer; // number of bytes in buffer
|
||||
|
||||
// ------------------------------- flags
|
||||
|
||||
mjtBool flg_gravcomp; // whether any body has nonzero gravcomp
|
||||
mjtBool flg_surfacevel; // whether any geom has nonzero surfacevel
|
||||
|
||||
// ------------------------------- options and statistics
|
||||
|
||||
mjOption opt; // physics options
|
||||
|
||||
@@ -247,7 +247,6 @@
|
||||
X( nnames_map ) \
|
||||
X( nJmom ) \
|
||||
X( ngravcomp ) \
|
||||
X( nsurfacevel ) \
|
||||
X( nemax ) \
|
||||
X( njmax ) \
|
||||
X( nconmax ) \
|
||||
|
||||
@@ -228,7 +228,7 @@ def fwd_actuation(m: Model, d: Data) -> Data:
|
||||
|
||||
qfrc_actuator = d._impl.actuator_moment.T @ force
|
||||
|
||||
if m.ngravcomp:
|
||||
if m.flg_gravcomp:
|
||||
# actuator-level gravity compensation, skip if added as passive force
|
||||
qfrc_actuator += d.qfrc_gravcomp * m.jnt_actgravcomp[m.dof_jntid]
|
||||
|
||||
|
||||
@@ -145,7 +145,7 @@ def passive(m: Model, d: Data) -> Data:
|
||||
qfrc_passive = _spring_damper(m, d)
|
||||
qfrc_gravcomp = jp.zeros(m.nv)
|
||||
|
||||
if m.ngravcomp and not m.opt.disableflags & DisableBit.GRAVITY:
|
||||
if m.flg_gravcomp and not m.opt.disableflags & DisableBit.GRAVITY:
|
||||
qfrc_gravcomp = _gravcomp(m, d)
|
||||
# add gravcomp unless added via actuators
|
||||
qfrc_passive += qfrc_gravcomp * (1 - m.jnt_actgravcomp[m.dof_jntid])
|
||||
|
||||
@@ -641,6 +641,8 @@ class Model(PyTreeNode):
|
||||
nJmom: int # pylint:disable=invalid-name
|
||||
nJten: int # pylint:disable=invalid-name
|
||||
ngravcomp: int
|
||||
flg_gravcomp: bool
|
||||
flg_surfacevel: bool
|
||||
nuserdata: int
|
||||
nsensordata: int
|
||||
npluginstate: int
|
||||
|
||||
@@ -169,6 +169,25 @@ class MuJoCoBindingsTest(parameterized.TestCase):
|
||||
self.data.qpos, [0.12345] * len(self.data.qpos)
|
||||
)
|
||||
|
||||
def test_flg_gravcomp_and_surfacevel_properties(self):
|
||||
self.assertFalse(self.model.flg_gravcomp)
|
||||
self.assertFalse(self.model.flg_surfacevel)
|
||||
self.assertEqual(self.model.ngravcomp, 0)
|
||||
|
||||
self.model.flg_gravcomp = True
|
||||
self.assertTrue(self.model.flg_gravcomp)
|
||||
self.assertEqual(self.model.ngravcomp, 1)
|
||||
|
||||
self.model.flg_surfacevel = True
|
||||
self.assertTrue(self.model.flg_surfacevel)
|
||||
|
||||
self.model.flg_gravcomp = False
|
||||
self.assertFalse(self.model.flg_gravcomp)
|
||||
self.assertEqual(self.model.ngravcomp, 0)
|
||||
|
||||
self.model.flg_surfacevel = False
|
||||
self.assertFalse(self.model.flg_surfacevel)
|
||||
|
||||
def test_array_is_a_view(self):
|
||||
qpos_ref = self.data.qpos
|
||||
self.data.qpos = 0.789
|
||||
|
||||
@@ -1368,11 +1368,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
type=ValueType(name='mjtSize'),
|
||||
doc='number of bodies with nonzero gravcomp',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='nsurfacevel',
|
||||
type=ValueType(name='mjtSize'),
|
||||
doc='number of geoms with nonzero surfacevel',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='nemax',
|
||||
type=ValueType(name='mjtSize'),
|
||||
@@ -1428,6 +1423,16 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
type=ValueType(name='mjtSize'),
|
||||
doc='number of bytes in buffer',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='flg_gravcomp',
|
||||
type=ValueType(name='mjtBool'),
|
||||
doc='whether any body has nonzero gravcomp',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='flg_surfacevel',
|
||||
type=ValueType(name='mjtBool'),
|
||||
doc='whether any geom has nonzero surfacevel',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='opt',
|
||||
type=ValueType(name='mjOption'),
|
||||
|
||||
@@ -350,6 +350,21 @@ This is useful for example when the MJB is not available as a file on disk.)"));
|
||||
mjModel.def_readonly("vis", &MjModelWrapper::vis);
|
||||
mjModel.def_readonly("stat", &MjModelWrapper::stat);
|
||||
|
||||
mjModel.def_property(
|
||||
"flg_gravcomp",
|
||||
[](const MjModelWrapper& m) { return m.get()->flg_gravcomp; },
|
||||
[](MjModelWrapper& m, bool val) {
|
||||
m.get()->flg_gravcomp = val;
|
||||
m.get()->ngravcomp = val ? 1 : 0;
|
||||
});
|
||||
|
||||
mjModel.def_property(
|
||||
"flg_surfacevel",
|
||||
[](const MjModelWrapper& m) { return m.get()->flg_surfacevel; },
|
||||
[](MjModelWrapper& m, bool val) {
|
||||
m.get()->flg_surfacevel = val;
|
||||
});
|
||||
|
||||
#define X(var) \
|
||||
mjModel.def_property_readonly( \
|
||||
#var, [](const MjModelWrapper& m) { return m.get()->var; });
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -1105,7 +1105,6 @@ public unsafe struct mjModel_ {
|
||||
public UInt64 nnames_map;
|
||||
public UInt64 nJmom;
|
||||
public UInt64 ngravcomp;
|
||||
public UInt64 nsurfacevel;
|
||||
public UInt64 nemax;
|
||||
public UInt64 njmax;
|
||||
public UInt64 nconmax;
|
||||
@@ -1117,6 +1116,8 @@ public unsafe struct mjModel_ {
|
||||
public UInt64 nhistory;
|
||||
public UInt64 narena;
|
||||
public UInt64 nbuffer;
|
||||
public byte flg_gravcomp;
|
||||
public byte flg_surfacevel;
|
||||
public mjOption_ opt;
|
||||
public mjVisual_ vis;
|
||||
public mjStatistic_ stat;
|
||||
|
||||
@@ -4982,6 +4982,8 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
||||
.property("flexvert_J_colind", &MjModel::flexvert_J_colind)
|
||||
.property("flexvert_J_rowadr", &MjModel::flexvert_J_rowadr)
|
||||
.property("flexvert_J_rownnz", &MjModel::flexvert_J_rownnz)
|
||||
.property("flg_gravcomp", &MjModel::flg_gravcomp, &MjModel::set_flg_gravcomp, reference())
|
||||
.property("flg_surfacevel", &MjModel::flg_surfacevel, &MjModel::set_flg_surfacevel, reference())
|
||||
.property("geom_aabb", &MjModel::geom_aabb)
|
||||
.property("geom_bodyid", &MjModel::geom_bodyid)
|
||||
.property("geom_conaffinity", &MjModel::geom_conaffinity)
|
||||
@@ -5210,7 +5212,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
|
||||
.property("nskinface", &MjModel::nskinface, &MjModel::set_nskinface, reference())
|
||||
.property("nskintexvert", &MjModel::nskintexvert, &MjModel::set_nskintexvert, reference())
|
||||
.property("nskinvert", &MjModel::nskinvert, &MjModel::set_nskinvert, reference())
|
||||
.property("nsurfacevel", &MjModel::nsurfacevel, &MjModel::set_nsurfacevel, reference())
|
||||
.property("ntendon", &MjModel::ntendon, &MjModel::set_ntendon, reference())
|
||||
.property("ntex", &MjModel::ntex, &MjModel::set_ntex, reference())
|
||||
.property("ntexdata", &MjModel::ntexdata, &MjModel::set_ntexdata, reference())
|
||||
|
||||
@@ -4242,12 +4242,6 @@ struct MjModel {
|
||||
void set_ngravcomp(int value) {
|
||||
ptr_->ngravcomp = static_cast<mjtSize>(value);
|
||||
}
|
||||
int nsurfacevel() const {
|
||||
return static_cast<int>(ptr_->nsurfacevel);
|
||||
}
|
||||
void set_nsurfacevel(int value) {
|
||||
ptr_->nsurfacevel = static_cast<mjtSize>(value);
|
||||
}
|
||||
int nemax() const {
|
||||
return static_cast<int>(ptr_->nemax);
|
||||
}
|
||||
@@ -4314,6 +4308,18 @@ struct MjModel {
|
||||
void set_nbuffer(int value) {
|
||||
ptr_->nbuffer = static_cast<mjtSize>(value);
|
||||
}
|
||||
mjtBool flg_gravcomp() const {
|
||||
return ptr_->flg_gravcomp;
|
||||
}
|
||||
void set_flg_gravcomp(mjtBool value) {
|
||||
ptr_->flg_gravcomp = value;
|
||||
}
|
||||
mjtBool flg_surfacevel() const {
|
||||
return ptr_->flg_surfacevel;
|
||||
}
|
||||
void set_flg_surfacevel(mjtBool value) {
|
||||
ptr_->flg_surfacevel = value;
|
||||
}
|
||||
emscripten::val buffer() const {
|
||||
return emscripten::val(emscripten::typed_memory_view(ptr_->nbuffer, static_cast<uint8_t*>(ptr_->buffer)));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user