Fix flex bending frame transformation when parent body is rotated.
PiperOrigin-RevId: 958362895 Change-Id: I0f6ba12a441bd7daa54292bfe2c70f0d836b008d
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
2c8d7b565d
commit
7927e908a3
@@ -1842,6 +1842,86 @@ static void stretchK_dense(mjModel* m, mjData* d, mjtNum* K, int nv,
|
||||
}
|
||||
}
|
||||
|
||||
// The same frame mismatch seen through the derivative: mjd_flexBend_mul is the
|
||||
// Jacobian of the bending force only if operator and force agree on the frame.
|
||||
// An unrotated flex cannot catch it, because the stencil is one scalar per
|
||||
// vertex pair applied coordinate-wise and so commutes with a rotation shared by
|
||||
// every vertex. The pin also covers the zero-dof vertex path, where body_dofadr
|
||||
// is negative and an unguarded stencil indexes out of bounds.
|
||||
TEST_F(DerivativeTest, FlexBendDerivativesRotated) {
|
||||
static const char* const kXml = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast" solver="CG"/>
|
||||
<worldbody>
|
||||
<body name="turned" euler="90 35 20">
|
||||
<flexcomp name="rot" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
|
||||
radius=".01" dim="2" mass="1">
|
||||
<pin id="0"/>
|
||||
<contact selfcollide="none" contype="0" conaffinity="0"/>
|
||||
<elasticity young="1e4" poisson="0.3" thickness="0.01"
|
||||
elastic2d="bend" damping="0"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
<flexcomp name="flat" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
|
||||
radius=".01" dim="2" mass="1" pos="1 0 0">
|
||||
<pin id="0"/>
|
||||
<contact selfcollide="none" contype="0" conaffinity="0"/>
|
||||
<elasticity young="1e4" poisson="0.3" thickness="0.01"
|
||||
elastic2d="bend" damping="0"/>
|
||||
</flexcomp>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||
int nv = model->nv;
|
||||
ASSERT_EQ(model->nq, nv); // all slide dofs
|
||||
ASSERT_EQ(model->nflex, 2);
|
||||
MjDataPtr data = MakeData(model);
|
||||
|
||||
// deform both flexes out of plane so the bending stencil carries real force
|
||||
for (int i = 0; i < nv; i++) {
|
||||
data->qpos[i] += 2e-3 * (mju_Halton(i, 2) - 0.5);
|
||||
}
|
||||
mj_forward(model.get(), data.get());
|
||||
|
||||
std::vector<mjtNum> vec(nv), res(nv, 0);
|
||||
for (int i = 0; i < nv; i++) {
|
||||
vec[i] = mju_Halton(i, 3) - 0.5;
|
||||
}
|
||||
mjd_flexBend_mul(model.get(), data.get(), res.data(), vec.data(), 1, 0);
|
||||
|
||||
mjtNum eps = MjTol(1e-7, 1e-4);
|
||||
mjData* perturbed = mj_copyData(NULL, model.get(), data.get());
|
||||
mju_addToScl(perturbed->qpos, vec.data(), eps, nv);
|
||||
mj_forward(model.get(), perturbed);
|
||||
|
||||
// check each flex on its own: the unrotated one is the control that isolates
|
||||
// the rotation
|
||||
for (int f = 0; f < model->nflex; f++) {
|
||||
SCOPED_TRACE(model->names + model->name_flexadr[f]);
|
||||
mjtNum max_err = 0, scale = 0;
|
||||
for (int k = 0; k < model->flex_vertnum[f]; k++) {
|
||||
int body = model->flex_vertbodyid[model->flex_vertadr[f] + k];
|
||||
int adr = model->body_dofadr[body];
|
||||
for (int x = 0; x < model->body_dofnum[body]; x++) {
|
||||
mjtNum fd = -(perturbed->qfrc_passive[adr+x] -
|
||||
data->qfrc_passive[adr+x]) / eps;
|
||||
max_err = mju_max(max_err, mju_abs(res[adr+x] - fd));
|
||||
scale = mju_max(scale, mju_abs(fd));
|
||||
}
|
||||
}
|
||||
// bending is linear in position, so the difference is exact up to roundoff
|
||||
EXPECT_GT(scale, 1e-3)
|
||||
<< "test should exercise nontrivial bending stiffness";
|
||||
EXPECT_LT(max_err, MjTol(1e-4, 1e-2) * scale)
|
||||
<< "mjd_flexBend_mul is not the Jacobian of the flex bending force";
|
||||
}
|
||||
mj_deleteData(perturbed);
|
||||
}
|
||||
|
||||
// verify mjd_flexStretch_mul (Gauss-Newton Hessian of the standard-flex
|
||||
// stretch force) against finite differences of qfrc_passive, plus symmetry,
|
||||
// positive semi-definiteness and (s1, s2) scale linearity. The model covers
|
||||
|
||||
@@ -1094,6 +1094,93 @@ TEST_F(ElasticityTest, TrilinearParentBodyRotation) {
|
||||
}
|
||||
|
||||
|
||||
// A dim=2 flexcomp with bending elasticity inside a parent body with a
|
||||
// non-identity quaternion. The implicit metric assembles the bending
|
||||
// stiffness from world-space vertex positions, but the vertex bodies' slide
|
||||
// dofs live in the (rotated) parent frame. Without the R^T (.) R change
|
||||
// of basis the metric stops being the Jacobian of the passive force,
|
||||
// which shows up as a loss of rotational invariance and, at stiffnesses
|
||||
// the unrotated model handles comfortably, as divergence.
|
||||
TEST_F(ElasticityTest, BendParentBodyRotation) {
|
||||
static constexpr char rotated_xml[] = R"(
|
||||
<mujoco>
|
||||
<option gravity="0 0 0" integrator="implicitfast"
|
||||
timestep="0.001" solver="CG"/>
|
||||
<worldbody>
|
||||
<body name="base" pos="0 0 0" quat="0.7071 0.7071 0 0">
|
||||
<flexcomp type="grid" count="5 5 1" spacing=".05 .05 .05"
|
||||
dim="2" radius=".001" mass=".01" name="sheet">
|
||||
<elasticity young="1e5" poisson="0" thickness="1e-2"
|
||||
elastic2d="bend"/>
|
||||
<contact selfcollide="none" internal="false"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
static constexpr char nonrotated_xml[] = R"(
|
||||
<mujoco>
|
||||
<option gravity="0 0 0" integrator="implicitfast"
|
||||
timestep="0.001" solver="CG"/>
|
||||
<worldbody>
|
||||
<body name="base" pos="0 0 0">
|
||||
<flexcomp type="grid" count="5 5 1" spacing=".05 .05 .05"
|
||||
dim="2" radius=".001" mass=".01" name="sheet">
|
||||
<elasticity young="1e5" poisson="0" thickness="1e-2"
|
||||
elastic2d="bend"/>
|
||||
<contact selfcollide="none" internal="false"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
char error[1024] = {0};
|
||||
|
||||
MjModelPtr m_rot = LoadModelFromString(rotated_xml, error, sizeof(error));
|
||||
ASSERT_THAT(m_rot.get(), NotNull()) << error;
|
||||
MjDataPtr d_rot = MakeData(m_rot);
|
||||
|
||||
MjModelPtr m_non = LoadModelFromString(nonrotated_xml, error, sizeof(error));
|
||||
ASSERT_THAT(m_non.get(), NotNull()) << error;
|
||||
MjDataPtr d_non = MakeData(m_non);
|
||||
|
||||
// lift one corner out of plane: that bends at first order but stretches only
|
||||
// at second, so the bending force is not swamped. As in the stretch case the
|
||||
// displacement is applied in the parent frame, so the response must not
|
||||
// depend on the parent's orientation
|
||||
d_rot->qpos[2] = 1e-2;
|
||||
d_non->qpos[2] = 1e-2;
|
||||
|
||||
mj_forward(m_rot.get(), d_rot.get());
|
||||
mj_forward(m_non.get(), d_non.get());
|
||||
|
||||
EXPECT_LT(d_non->qfrc_passive[2], -1e-6)
|
||||
<< "expected a restoring force on the lifted dof";
|
||||
|
||||
const mjtNum tol = MjTol(1e-12, 1e-5);
|
||||
for (int i = 0; i < m_rot->nv; i++) {
|
||||
EXPECT_NEAR(d_rot->qfrc_passive[i], d_non->qfrc_passive[i], tol)
|
||||
<< "rotated/non-rotated qfrc mismatch at dof " << i;
|
||||
}
|
||||
|
||||
// qacc exercises the metric itself, so this also covers mjd_flexBend_mul
|
||||
for (int i = 0; i < m_rot->nv; i++) {
|
||||
EXPECT_NEAR(d_rot->qacc[i], d_non->qacc[i], MjTol(1e-9, 1e-3))
|
||||
<< "rotated/non-rotated qacc mismatch at dof " << i;
|
||||
}
|
||||
|
||||
// and the rotated model must integrate stably
|
||||
for (int step = 0; step < 200; step++) {
|
||||
mj_step(m_rot.get(), d_rot.get());
|
||||
for (int i = 0; i < m_rot->nv; i++) {
|
||||
ASSERT_TRUE(std::isfinite(d_rot->qacc[i]))
|
||||
<< "NaN/Inf in qacc at dof " << i << " at step " << step;
|
||||
}
|
||||
if (HasFatalFailure()) return;
|
||||
}
|
||||
}
|
||||
|
||||
// A dim=2 flexcomp with stretch elasticity inside a parent body with a
|
||||
// non-identity quaternion. The implicit metric assembles the stretch
|
||||
// stiffness from world-space edge vectors, but the vertex bodies' slide
|
||||
|
||||
Reference in New Issue
Block a user