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

Add per-body gravity compensation (buoyancy) passive force.

Browse Source 
PiperOrigin-RevId: 485342531
Change-Id: Icb3e6bb5080b6ef17a3d3b1cc67e0503cfeaeffb
This commit is contained in:
Kyle Bayes
2022-11-01 09:47:13 -07:00
committed by Copybara-Service
parent c7960385be
commit 23092a11d7
18 changed files with 233 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference.rst
+1
View File
@@ -1464,6 +1464,7 @@ mjModel
mjtNum* body_subtreemass; // mass of subtree starting at this body (nbody x 1)
mjtNum* body_inertia; // diagonal inertia in ipos/iquat frame (nbody x 3)
mjtNum* body_invweight0; // mean inv inert in qpos0 (trn, rot) (nbody x 2)
mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1)
mjtNum* body_user; // user data (nbody x nuser_body)
int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1)
doc/XMLreference.rst
+5
View File
@@ -1690,6 +1690,11 @@ defined. Its body name is automatically defined as "world".
unit quaternion, thus specifying it is optional even in local coordinates. If the body frame was copied from the body
inertial frame per the above rules, the copy operation applies to both position and orientation, and the setting of
the orientation-related attributes is ignored.
:at:`gravcomp`: :at-val:`real, "0"`
Gravity compensation force, specified as fraction of body weight. This attribute creates an upwards force applied to
the body's center of mass, countering the force of gravity. As an example, a value of ``1`` creates an upward force
equal to the body's weight and compensates for gravity exactly. Values greater than ``1`` will create a net upwards
force or buoyancy effect.
:at:`user`: :at-val:`real(nbody_user), "0 0 ..."`
See :ref:`CUser`.
doc/XMLschema.rst
+1 -1
View File
@@ -585,7 +585,7 @@
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`xyaxes` | :at:`zaxis` | :at:`euler` | |
| | | +-------------------------+-------------------------+-------------------------+ |
| | | | :at:`user` | | | |
| | | | :at:`gravcomp` | :at:`user` | | |
| | | +-------------------------+-------------------------+-------------------------+ |
+--------------------------+----+------------------------------------------------------------------------------------+
| |_2|:el:`plugin` | \* | .. table:: |
doc/changelog.rst
+6
View File
@@ -21,8 +21,14 @@ General
This is a minor breaking API change. ``mjModel.tendon_lengthspring`` now has size ``ntendon x 2`` rather than
``ntendon x 1``.
.. youtube:: -PJ6afdETUg
:align: right
:height: 150px
- Removed the requirement that stateless actuators come before stateful actuators.
- Added :ref:`mju_fill`, :ref:`mju_symmetrize` and :ref:`mju_eye` utility functions.
- Added :at:`gravcomp` attribute to :ref:`body<body>`, implementing gravity compensation and bouyancy.
See `balloons.xml <https://github.com/deepmind/mujoco/tree/main/model/balloons/balloons.xml>`_ example model.
Version 2.3.0 (October 18, 2022)
--------------------------------
doc/computation.rst
+12 -7
View File
@@ -359,17 +359,22 @@ by MuJoCo are also passive in the sense of physics, i.e., they do not increase e
callback :ref:`mjcb_passive` and add forces to ``mjData.qfrc_passive`` that may increase energy. This will not interfere
with MuJoCo's operation as long as such user forces depend only on position and velocity.
MuJoCo can compute two types of passive forces: spring-dampers in joints and tendons, and fluid dynamics. When Euler
integration is used, joint damping is integrated implicitly (by modifying the inertia matrix internally) which
significantly increases stability. Thus, even though damping can be alternatively modeled as an actuator property, it is
better to model it as a joint property. Note also the XML :ref:`joint <body-joint>` attribute springdamper which
automates the creation of mass-spring-dampers with desired time constants and damping ratios; in that case the compiler
computes the stiffness and damping coefficients of the joint by taking the joint inertia into account.
MuJoCo can compute three types of passive forces: spring-dampers in joints and tendons, gravity compensation forces, and
fluid dynamics.
When Euler or the implicit integator are used, joint damping is integrated implicitly which significantly increases
stability. Thus, even though damping can be modeled as an actuator property, it is better to model it as a joint
property. Note also the XML :ref:`joint <body-joint>` attribute springdamper which automates the creation of mass-
spring-dampers with desired time constants and damping ratios; in that case the compiler computes the stiffness and
damping coefficients of the joint by taking the joint inertia into account.
Gravity compensation is a force applied to a body's center of mass opposing gravity, see :ref:`body gravcomp<body>` for
details.
Proper simulation of fluid dynamics is beyond the scope of MuJoCo, and would be too slow for the applications we aim to
facilitate. Nevertheless we provide a phenomenological model which is sufficient for simulating behaviors such as flying
and swimming. It is enabled by setting ``mjModel.opt.viscosity`` and ``mjModel.opt.density`` to positive values (they
are zero by default.) These parameters specify the viscosity :math:`\beta` and density :math:`\rho` of the medium and
are zero by default). These parameters specify the viscosity :math:`\beta` and density :math:`\rho` of the medium and
apply to all bodies. The shape of each body for fluid dynamics purposes is assumed to be the equivalent inertia box,
which can also be visualized. Each forward-facing (relative to the linear velocity) face of the box experiences force
along its normal direction. All faces also experience torque due to the angular velocity; this torque is obtained by
include/mujoco/mjmodel.h
+1
View File
@@ -631,6 +631,7 @@ struct mjModel_ {
mjtNum* body_subtreemass; // mass of subtree starting at this body (nbody x 1)
mjtNum* body_inertia; // diagonal inertia in ipos/iquat frame (nbody x 3)
mjtNum* body_invweight0; // mean inv inert in qpos0 (trn, rot) (nbody x 2)
mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1)
mjtNum* body_user; // user data (nbody x nuser_body)
int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1)
include/mujoco/mjxmacro.h
+1
View File
@@ -170,6 +170,7 @@
X( mjtNum, body_subtreemass, nbody, 1 ) \
X( mjtNum, body_inertia, nbody, 3 ) \
X( mjtNum, body_invweight0, nbody, 2 ) \
X( mjtNum, body_gravcomp, nbody, 1 ) \
X( mjtNum, body_user, nbody, MJ_M(nuser_body) ) \
X( int, body_plugin, nbody, 1 ) \
X( int, jnt_type, njnt, 1 ) \
model/balloons/balloons.xml
+108
View File
@@ -0,0 +1,108 @@
<mujoco>
<option density="1.204" viscosity="1.8e-5" integrator="implicit"/>
<visual>
<global elevation="-10"/>
</visual>
<default>
<tendon limited="true" width="0.003" rgba="1 1 1 1"/>
<geom friction=".2"/>
<default class="weight">
<geom rgba=".8 .4 .8 1"/>
<site rgba=".8 .4 .8 1"/>
</default>
<default class="balloon">
<!--
0.167 is the MKS density of helium at room temperature.
Note this does not take into account the mass of the rubber,
which is likely not insignificant.
-->
<geom density="0.167" fluidshape="ellipsoid"/>
<default class="pink">
<geom rgba="1 .6 .7 1"/>
<site rgba="1 .6 .7 1"/>
</default>
<default class="blue">
<geom rgba=".3 .7 .9 1"/>
<site rgba=".3 .7 .9 1"/>
</default>
<default class="green">
<geom rgba=".4 .9 .5 1"/>
<site rgba=".4 .9 .5 1"/>
</default>
<default class="orange">
<geom rgba="1 .4 0 1"/>
<site rgba="1 .4 0 1"/>
</default>
</default>
</default>
<asset>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb2="0 0 0" rgb1="1 1 1"/>
<material name="grid" texture="grid" texrepeat="2 2" texuniform="true" reflectance=".6"/>
</asset>
<worldbody>
<geom name="ground" type="plane" size="5 5 .05" pos="0 0 -.5" material="grid"/>
<geom name="ramp" type="box" size=".4 .2 .03" pos="0 0 -.4" euler="0 20 0" rgba="1 1 1 1"/>
<body name="weight" childclass="weight" pos=".3 0 .2">
<freejoint/>
<light pos="1 0 3" dir="-1 0 -3" mode="trackcom"/>
<light pos="-1 0 3" dir="1 0 -3" mode="trackcom"/>
<!-- The mass of the weight was chosen to be slightly bigger than the total buoyancy of the balloons. -->
<geom name="weight" type="box" size=".015 .015 .015" mass=".0347"/>
<site name="weight1" pos=" .013 .013 .013" size="0.005"/>
<site name="weight2" pos="-.013 -.013 .013" size="0.005"/>
</body>
<!-- The gravcomp value of 7.2 is the ratio of air and helium desities at room temperature. -->
<body name="pink" gravcomp="7.2" pos="-.2 .1 .2" childclass="pink">
<freejoint />
<geom name="pink" type="ellipsoid" size=".11 .11 .15"/>
<geom name="pink_knot" pos="0 0 -.15" size=".02"/>
<site name="pink" pos="0 0 -.17" size="0.01"/>
</body>
<body name="blue" gravcomp="7.2" pos=".1 .1 .2" childclass="blue">
<freejoint />
<geom name="blue" type="ellipsoid" size=".12 .12 .15"/>
<geom name="blue_knot" pos="0 0 -.15" size=".02"/>
<site name="blue" pos="0 0 -.17" size="0.01"/>
</body>
<body name="green" gravcomp="7.2" pos=".1 -.1 .2" childclass="green">
<freejoint />
<geom name="green" type="ellipsoid" size=".12 .12 .14"/>
<geom name="green_knot" pos="0 0 -.14" size=".02"/>
<site name="green" pos="0 0 -.16" size="0.01"/>
</body>
<body name="orange" gravcomp="7.2" pos="-.12 -.12 .2" childclass="orange">
<freejoint />
<geom name="orange" type="ellipsoid" size=".12 .12 .13"/>
<geom name="orange_knot" pos="0 0 -.13" size=".02"/>
<site name="orange" pos="0 0 -.15" size="0.01"/>
</body>
</worldbody>
<tendon>
<spatial range="0 0.6">
<site site="pink"/>
<site site="weight1"/>
</spatial>
<spatial range="0 0.4">
<site site="blue"/>
<site site="weight1"/>
</spatial>
<spatial range="0 0.3">
<site site="green"/>
<site site="weight2"/>
</spatial>
<spatial range="0 0.5">
<site site="orange"/>
<site site="weight2"/>
</spatial>
</tendon>
</mujoco>
src/engine/engine_core_smooth.c
+13
View File
@@ -1409,6 +1409,19 @@ void mj_passive(const mjModel* m, mjData* d) {
}
}
// body-level gravity compensation
if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
mjtNum force[3], torque[3]={0};
// apply per-body gravity compensation
for (int i=1; i<m->nbody; i++) {
if (m->body_gravcomp[i]) {
mju_scl3(force, m->opt.gravity, -(m->body_mass[i]*m->body_gravcomp[i]));
mj_applyFT(m, d, force, torque, d->xipos+3*i, i, d->qfrc_passive);
}
}
}
// body-level viscosity, lift and drag
if (m->opt.viscosity>0 || m->opt.density>0) {
for (int i=1; i<m->nbody; i++) {
src/user/user_model.cc
+1
View File
@@ -1285,6 +1285,7 @@ void mjCModel::CopyTree(mjModel* m) {
copyvec(m->body_iquat+4*i, pb->lociquat, 4);
m->body_mass[i] = (mjtNum)pb->mass;
copyvec(m->body_inertia+3*i, pb->inertia, 3);
m->body_gravcomp[i] = pb->gravcomp;
copyvec(m->body_user+nuser_body*i, pb->userdata.data(), nuser_body);
// count free joints
src/user/user_objects.cc
+1
View File
@@ -326,6 +326,7 @@ mjCBody::mjCBody(mjCModel* _model) {
weldid = -1;
dofnum = 0;
lastdof = -1;
gravcomp = 0;
userdata.clear();
// plugin variables
src/user/user_objects.h
+1
View File
@@ -186,6 +186,7 @@ class mjCBody : public mjCBase {
double iquat[4]; // inertial frame orientation
double mass; // mass
double inertia[3]; // diagonal inertia (in i-frame)
double gravcomp; // gravity compensation
std::vector<double> userdata; // user data
mjCAlternative alt; // alternative orientation specification
mjCAlternative ialt; // alternative for inertial frame
src/xml/xml_native_reader.cc
+5 -2
View File
@@ -227,8 +227,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"emission", "specular", "shininess", "reflectance", "rgba"},
{">"},
{"body", "R", "10", "name", "childclass", "pos", "quat", "mocap",
"axisangle", "xyaxes", "zaxis", "euler", "user"},
{"body", "R", "11", "name", "childclass", "pos", "quat", "mocap",
"axisangle", "xyaxes", "zaxis", "euler", "gravcomp", "user"},
{"<"},
{"plugin", "*", "3", "name", "plugin", "instance"},
{"<"},
@@ -2714,6 +2714,9 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
}
ReadAlternative(elem, pchild->alt);
// read gravcomp
ReadAttr(elem, "gravcomp", 1, &pchild->gravcomp, text);
// read userdata
ReadVector(elem, "user", pchild->userdata, text);
src/xml/xml_native_writer.cc
+3
View File
@@ -1338,6 +1338,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
WriteAttrKey(elem, "mocap", bool_map, 2, 1);
}
// gravity compensation
WriteAttr(elem, "gravcomp", 1, &body->gravcomp);
// userdata
WriteVector(elem, "user", body->userdata);
test/engine/engine_forward_test.cc
+42
View File
@@ -43,6 +43,7 @@ using ::testing::Pointwise;
using ::testing::DoubleNear;
using ::testing::Ne;
using ::testing::HasSubstr;
using ::testing::NotNull;
// --------------------------- activation limits -------------------------------
@@ -387,5 +388,46 @@ TEST_F(ForwardTest, MjcbControlDisabled) {
mj_deleteModel(model);
}
TEST_F(ForwardTest, gravcomp) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -10" />
<worldbody>
<body>
<joint type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
<body pos="3 0 0" gravcomp="1">
<joint type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
<body pos="6 0 0" gravcomp="2">
<joint type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
while(data->time < 1) { mj_step(model, data); }
mjtNum dist = 0.5*mju_norm3(model->opt.gravity)*(data->time*data->time);
// expect that body 1 moves down allowing some slack from our estimated distance moved
EXPECT_NEAR(data->qpos[0], -dist, 0.011);
// expect that body 2 does not move
EXPECT_EQ(data->qpos[1], 0.0);
// expect that body 3 moves up the same distance that body 0 moved down
EXPECT_EQ(data->qpos[0], -data->qpos[2]);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/xml/xml_native_reader_test.cc
+15
View File
@@ -361,6 +361,21 @@ TEST_F(XMLReaderTest, AllowsSpaces) {
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, gravcomp) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body gravcomp="1.01">
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
EXPECT_EQ(model->body_gravcomp[1], 1.01);
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, InvalidDoubleOrientation) {
std::string prefix = "<mujoco><worldbody><";
std::string suffix = "/></worldbody></mujoco>";
test/xml/xml_native_writer_test.cc
+16
View File
@@ -557,6 +557,22 @@ TEST_F(XMLWriterTest, KeepsForcelimitedFalse) {
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, WritesGravComp) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body gravcomp=".25">
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, HasSubstr("gravcomp=\"0.25\""));
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, UndefinedMassDensity) {
static constexpr char xml[] = R"(
<mujoco>
unity/Runtime/Bindings/MujocoBindings.cs
+1
View File
@@ -1946,6 +1946,7 @@ public unsafe struct mjModel_ {
public double* body_subtreemass;
public double* body_inertia;
public double* body_invweight0;
public double* body_gravcomp;
public double* body_user;
public int* body_plugin;
public int* jnt_type;