Skip to content

Extend gyroscopic stabilization to free rigid subtrees #3554

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
ruziniuuuuu wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Extend gyroscopic stabilization to free rigid subtrees #3554

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion doc/computation/index.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -614,7 +614,8 @@ Gyroscopic derivatives for free bodies
with the faster Cholesky decomposition. However integrating gyroscopic forces explicitly can lead to
energy gain and divergence of fast-spinning free bodies with asymmetric inertia.

Therefore for *standalone free bodies* (free joints whose body has no children), these derivatives are reinstated.
Therefore for *standalone free bodies* (free joints whose body has no descendants with joints), these derivatives
are reinstated. Fixed descendants are included in the rigid body's inertia and bias derivatives.
The rows of :math:`\widehat M` corresponding to such a body form a :math:`6\times 6` block which is decoupled from
the rest of the system. After the global Cholesky solve, this block is re-assembled with the exact derivative of the
body's bias force and re-solved with an optimized :math:`6\times 6` LU routine. For standalone free bodies,
Expand Down
69 changes: 25 additions & 44 deletions src/engine/engine_derivative.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -789,36 +789,34 @@ static void freeBias_vel_blocks(mjtNum mass, const mjtNum R[9], const mjtNum Xi[
}


// 6x6 block B = d qfrc_bias / d qvel for a standalone free body
// 6x6 block B = d qfrc_bias / d qvel for a free rigid subtree
// assembles the full 6x6 from the 3x3 sub-blocks computed by freeBias_vel_blocks
// rows/cols ordered like the free joint dofs: [linear(3); rotational(3)]
// linear columns are zero: the bias force does not depend on linear velocity
void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt, mjtNum B[36]) {
int body = m->jnt_bodyid[jnt];
int adr = m->jnt_dofadr[jnt];
mjtNum mass = m->body_mass[body];
const mjtNum* R = d->xmat + 9*body; // body -> world
const mjtNum* Xi = d->ximat + 9*body; // inertia -> world
const mjtNum* inertia = m->body_inertia + 3*body;

// CoM offset from joint origin, world frame
mjtNum s[3];
mji_sub3(s, d->xipos + 3*body, d->xpos + 3*body);
mju_zero(B, 36);

mjtNum lin[9], rot[9];
freeBias_vel_blocks(mass, R, Xi, inertia, s, d->qvel + adr + 3, lin, rot);
// fixed descendants share the free joint's angular velocity and reference frame
for (int i=body; i < m->nbody && m->body_rootid[i] == body; i++) {
mjtNum mass = m->body_mass[i];
mjtNum s[3], lin[9], rot[9];
mji_sub3(s, d->xipos + 3*i, d->xpos + 3*body);
freeBias_vel_blocks(mass, d->xmat + 9*body, d->ximat + 9*i,
m->body_inertia + 3*i, s, d->qvel + adr + 3, lin, rot);

mju_zero(B, 36);
for (int r=0; r < 3; r++) {
for (int c=0; c < 3; c++) {
B[6*r + 3+c] = -mass * lin[3*r+c];
B[6*(3+r) + 3+c] = rot[3*r+c];
for (int r=0; r < 3; r++) {
for (int c=0; c < 3; c++) {
B[6*r + 3+c] -= mass * lin[3*r+c];
B[6*(3+r) + 3+c] += rot[3*r+c];
}
}
}
}


// return 1 if body is a standalone free body (single free joint, no children)
// return 1 if body is the root of a free rigid subtree (single free joint, fixed descendants)
mjtBool mj_isFreeBody(const mjModel* m, int body) {
// must have exactly one joint, of free type
if (m->body_jntnum[body] != 1 || m->jnt_type[m->body_jntadr[body]] != mjJNT_FREE) {
Expand All @@ -827,26 +825,21 @@ mjtBool mj_isFreeBody(const mjModel* m, int body) {

int adr = m->jnt_dofadr[m->body_jntadr[body]];

// must be a standalone 6-DOF tree with no children
if (m->tree_dofnum[m->dof_treeid[adr]] != 6 ||
m->body_subtreemass[body] != m->body_mass[body]) {
return false;
}

return true;
// descendants must not add degrees of freedom
return m->tree_dofnum[m->dof_treeid[adr]] == 6;
}


// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body
// returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise
// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a rigid subtree
// returns 1 and writes A if jnt is the free joint of an awake rigid subtree, 0 otherwise
// requires valid d->qDeriv rows for the block, computed with flg_bias = 0; the bias
// derivative excluded from qDeriv is added here via mjd_freeBias_vel
int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[36],
int flg_discrete) {
int body = m->jnt_bodyid[jnt];
int adr = m->jnt_dofadr[jnt];

// must be a standalone free body, awake
// must be a free rigid subtree, awake
if (!mj_isFreeBody(m, body) || !d->tree_awake[m->dof_treeid[adr]]) {
return 0;
}
Expand Down Expand Up @@ -902,21 +895,9 @@ int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[

// A -= h * d(qfrc_smooth)/d(qvel) for the bias term missing from qDeriv;
// qfrc_smooth includes -qfrc_bias, so subtracting its derivative adds +h*B
mjtNum s[3];
mji_sub3(s, d->xipos + 3*body, d->xpos + 3*body);

mjtNum mass = m->body_mass[body];
mjtNum lin[9], rot[9];
freeBias_vel_blocks(mass, d->xmat + 9*body, d->ximat + 9*body,
m->body_inertia + 3*body, s, d->qvel + adr + 3, lin, rot);

mjtNum h_mass = -h * mass;
for (int r=0; r < 3; r++) {
for (int c=0; c < 3; c++) {
A[6*r + 3+c] += h_mass * lin[3*r+c];
A[6*(3+r) + 3+c] += h * rot[3*r+c];
}
}
mjtNum B[36];
mjd_freeBias_vel(m, d, jnt, B);
mju_addToScl(A, B, h, 36);

return 1;
}
Expand Down Expand Up @@ -2845,9 +2826,9 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, mjtNum* res, int bodyid,
}

// make B symmetric for the metric, or if integrator is IMPLICITFAST,
// except for standalone free bodies
// except for free rigid subtrees, including fluid geoms on fixed descendants
if (flg_dragonly ||
(m->opt.integrator == mjINT_IMPLICITFAST && !mj_isFreeBody(m, bodyid))) {
(m->opt.integrator == mjINT_IMPLICITFAST && !mj_isFreeBody(m, m->body_rootid[bodyid]))) {
mju_symmetrize(B, B, 6);
}

Expand Down
8 changes: 4 additions & 4 deletions src/engine/engine_derivative.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,14 +43,14 @@ MJAPI void mjd_passive_vel(const mjModel* m, mjData* d);
// subtract (d qfrc_bias / d qvel) from qDeriv (dense version)
MJAPI void mjd_rne_vel_dense(const mjModel* m, mjData* d);

// return 1 if body is a standalone free body: a free joint with no children
// return 1 if body is the root of a free rigid subtree: a free joint with fixed descendants
mjtBool mj_isFreeBody(const mjModel* m, int body);

// 6x6 block B = d qfrc_bias / d qvel for the free joint of a standalone body
// 6x6 block B = d qfrc_bias / d qvel for the free joint of a rigid subtree
MJAPI void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt, mjtNum B[36]);

// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body
// returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise
// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a rigid subtree
// returns 1 and writes A if jnt is the free joint of an awake rigid subtree, 0 otherwise
// requires valid d->qDeriv rows for the block, computed with flg_bias = 0
MJAPI int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[36],
int flg_discrete);
Expand Down
96 changes: 96 additions & 0 deletions test/engine/engine_derivative_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -222,6 +222,102 @@ TEST_F(DerivativeTest, FreeBiasVel) {
}
}

// fixed descendants contribute inertia about the free joint, not their own body
// frames
TEST_F(DerivativeTest, FreeBiasVelFixedDescendants) {
static constexpr char xml[] = R"(
<mujoco>
<default><geom contype="0" conaffinity="0"/></default>
<worldbody>
<body pos=".1 -.2 .3" euler="20 -30 40">
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="2" pos=".04 -.02 .03" euler="10 20 30"/>
<body pos=".2 -.1 .3" euler="30 20 -10">
<geom type="box" size=".2 .1 .1" mass="1" pos=".03 .01 -.02"/>
<body pos="-.1 .2 .1" euler="10 -20 30">
<geom type="box" size=".1 .1 .2" mass=".5"/>
</body>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
ASSERT_EQ(model->nbody, 4);
MjDataPtr data = MakeData(model);
mjModel* m = model.get();
mjData* d = data.get();
mjtNum qvel[6] = {0.4, -0.3, 0.2, 5, -3, 2};
mju_copy(d->qvel, qvel, 6);
mj_forward(m, d);

mjtNum B[36];
mjd_freeBias_vel(m, d, /*jnt=*/0, B);

// compare the aggregate derivative with the full recursive Newton-Euler
// derivative
mju_zero(d->qDeriv, m->nD);
mjd_smooth_vel(m, d, /*flg_bias=*/1);
for (int r = 0; r < 6; r++) {
ASSERT_EQ(m->D_rownnz[r], 6);
for (int k = 0; k < 6; k++) {
int adr = m->D_rowadr[r] + k;
EXPECT_NEAR(B[6 * r + m->D_colind[adr]], -d->qDeriv[adr],
MjTol(1e-14, 1e-5));
}
}

// independent central finite differences, including the zero linear-velocity
// columns
mjtNum eps = MjEps(1e-6, 1e-3);
for (int c = 0; c < 6; c++) {
mjtNum plus[6], minus[6];
d->qvel[c] = qvel[c] + eps;
mj_comVel(m, d);
mj_rne(m, d, /*flg_acc=*/0, plus);
d->qvel[c] = qvel[c] - eps;
mj_comVel(m, d);
mj_rne(m, d, /*flg_acc=*/0, minus);
d->qvel[c] = qvel[c];
for (int r = 0; r < 6; r++) {
EXPECT_NEAR(B[6 * r + c], (plus[r] - minus[r]) / (2 * eps),
MjTol(1e-7, 1e-2));
}
}
}

// a jointed descendant must still exclude the free root from the local six-DOF
// solve
TEST_F(DerivativeTest, FreeMhatRejectsArticulatedSubtree) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<freejoint/>
<geom size=".1"/>
<body pos="0 0 1">
<joint/>
<geom size=".1"/>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum A[36];
for (int discrete : {0, 1}) {
EXPECT_EQ(mjd_freeMhat(model.get(), data.get(), /*jnt=*/0,
model->opt.timestep, A, discrete),
0);
}
}

// disabled actuators do not contribute to d_qfrc_actuator/d_qvel
TEST_F(DerivativeTest, DisabledActuators) {
// model with only a position actuator
Expand Down
83 changes: 82 additions & 1 deletion test/engine/engine_forward_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -500,8 +500,47 @@ TEST_F(ImplicitIntegratorTest, FreeBodyMatchesImplicit) {
</mujoco>
)";

// free rigid subtree with rotated, offset inertias and multiple levels
static constexpr char xml3[] = R"(
<mujoco>
<option timestep="0.005"/>
<default><geom contype="0" conaffinity="0"/></default>
<worldbody>
<body pos="0.1 -0.2 0.5" euler="20 -30 40">
<joint type="free" damping="0.1"/>
<geom type="box" size=".1 .2 .3" mass="2" pos=".04 -.02 .03" euler="10 20 30"/>
<body pos=".2 -.1 .3" euler="30 20 -10">
<geom type="box" size=".2 .1 .1" mass="1" pos=".03 .01 -.02"/>
<body pos="-.1 .2 .1" euler="10 -20 30">
<geom type="box" size=".1 .1 .2" mass=".5"/>
</body>
</body>
</body>
<body pos="3 0 1">
<freejoint/>
<geom type="box" size=".1 .2 .3"/>
</body>
</worldbody>
</mujoco>
)";

// fluid forces on a fixed child must retain asymmetric lift derivatives
static constexpr char xml4[] = R"(
<mujoco>
<option timestep="0.005" density="1.2" viscosity="0.002" wind="1 2 3"/>
<worldbody>
<body pos="0.1 -0.2 0.5" euler="20 -30 40">
<freejoint/>
<body pos=".04 -.02 .03" euler="10 20 30">
<geom type="ellipsoid" size=".1 .2 .3" mass="2" fluidshape="ellipsoid"/>
</body>
</body>
</worldbody>
</mujoco>
)";

int xml_idx = 1;
for (auto xml : {xml1, xml2}) {
for (auto xml : {xml1, xml2, xml3, xml4}) {
SCOPED_TRACE(testing::Message() << "XML case " << xml_idx++);
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
Expand Down Expand Up @@ -579,6 +618,48 @@ TEST_F(ImplicitIntegratorTest, FreeBodyGyroStable) {
}
}

// a massless free root with a fixed inertial child must not disable gyroscopic
// stabilization
TEST_F(ImplicitIntegratorTest, FreeRigidSubtreeGyroStable) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" timestep="0.001" gravity="0 0 0">
<flag energy="enable"/>
</option>
<worldbody>
<body>
<freejoint/>
<body>
<inertial pos="0 0 0" mass="0.12"
diaginertia="0.00017231 0.00000658 0.00017243"/>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
ASSERT_EQ(model->nbody, 3);
MjDataPtr data = MakeData(model);
for (mjtIntegrator integrator : {mjINT_IMPLICITFAST, mjINT_DISCRETE}) {
SCOPED_TRACE(testing::Message() << "integrator " << integrator);
model->opt.integrator = integrator;
mj_resetData(model.get(), data.get());
data->qvel[3] = 100;
data->qvel[4] = 30;
data->qvel[5] = 100;
mj_forward(model.get(), data.get());
mjtNum initial_energy = data->energy[1];

for (int i = 0; i < 10000; i++) {
mj_step(model.get(), data.get());
ASSERT_EQ(data->warning[mjWARN_BADQACC].number, 0) << "step " << i;
ASSERT_LT(data->energy[1], 1.01 * initial_energy) << "step " << i;
}
}
}

// free-body local solve: applies to bodies in contact
TEST_F(ImplicitIntegratorTest, FreeBodyGyroStableContact) {
// spinning ellipsoid on an inclined plane, as in gyroscopic.xml
Expand Down