Fix flexcomp strain constraints with rotated grids.
The reference node positions and the positions used for computing stiffness eigenvectors were previously stored in world frame. However, the runtime expects these quantities in the unrotated local frame. This caused non-zero constraint residuals and simulation instability when the grid was rotated — either by the parent body's initial orientation, or by the flexcomp's own frame attributes. Rather than tracking each rotation source individually, this change extracts the total grid rotation directly from the cell geometry. All node positions are then un-rotated before computing the stiffness matrix. PiperOrigin-RevId: 903232388 Change-Id: If877af89025ce1e61a76b38c29403d593d892749
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
da01bd37a2
commit
ba149aa043
+80
-3
@@ -4251,6 +4251,9 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
}
|
||||
}
|
||||
|
||||
// compute unrotated node positions for stiffness computation
|
||||
std::vector<double> nodexpos_local = ComputeUnrotatedNodePositions(nodexpos);
|
||||
|
||||
// reorder tetrahedra so right-handed face orientation is outside
|
||||
// faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2)
|
||||
if (dim == 3) {
|
||||
@@ -4410,7 +4413,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
int gj = cj * spec.order + lj;
|
||||
int gk = ck * spec.order + lk;
|
||||
int global = gi * ny_global * nz_global + gj * nz_global + gk;
|
||||
mjuu_copyvec(cell_pos.data() + 3*local, nodexpos.data() + 3*global, 3);
|
||||
mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3);
|
||||
local++;
|
||||
}
|
||||
}
|
||||
@@ -4453,14 +4456,88 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
}
|
||||
}
|
||||
|
||||
// store node cartesian positions
|
||||
// store node positions in unrotated (body-local) frame
|
||||
// this ensures the runtime displacement refpos - R^{-1}*x is zero at rest
|
||||
node0_.assign(3*nnode, 0);
|
||||
for (int i=0; i < nnode; i++) {
|
||||
mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3);
|
||||
mjuu_copyvec(node0_.data()+3*i, nodexpos_local.data()+3*i, 3);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// compute unrotated node positions for stiffness computation and node0_
|
||||
//
|
||||
// the runtime corotational code extracts rotation R from the deformation
|
||||
// gradient and computes displacement as R^{-1}*x - refpos; at rest R = R0
|
||||
// (the total grid rotation), so refpos must equal R0^{-1}*nodexpos to get
|
||||
// zero displacement at rest; additionally, the stiffness eigenvectors must
|
||||
// be computed from axis-aligned positions to preserve the diagonal Jacobian
|
||||
// assumption in ComputeLinearStiffness.
|
||||
std::vector<double> mjCFlex::ComputeUnrotatedNodePositions(
|
||||
const std::vector<double>& nodexpos) const {
|
||||
std::vector<double> nodexpos_local(3*nnode);
|
||||
if (interpolated && nnode > 0) {
|
||||
int ny_global = spec.cellcount[1] * spec.order + 1;
|
||||
int nz_global = spec.cellcount[2] * spec.order + 1;
|
||||
|
||||
// find first non-empty cell
|
||||
int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2];
|
||||
int ref_ci = 0, ref_cj = 0, ref_ck = 0;
|
||||
bool found = false;
|
||||
for (int ci = 0; ci < cx && !found; ci++) {
|
||||
for (int cj = 0; cj < cy && !found; cj++) {
|
||||
for (int ck = 0; ck < cz && !found; ck++) {
|
||||
int cell_idx = ci * cy * cz + cj * cz + ck;
|
||||
if (cell_empty.empty() || !cell_empty[cell_idx]) {
|
||||
ref_ci = ci; ref_cj = cj; ref_ck = ck;
|
||||
found = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// corner indices of the reference cell (order=1 corners at local 0,0,0
|
||||
// and at offsets along each parametric axis)
|
||||
int g000 = (ref_ci * spec.order) * ny_global * nz_global +
|
||||
(ref_cj * spec.order) * nz_global +
|
||||
(ref_ck * spec.order);
|
||||
int g100 = ((ref_ci * spec.order) + spec.order) * ny_global * nz_global +
|
||||
(ref_cj * spec.order) * nz_global +
|
||||
(ref_ck * spec.order);
|
||||
int g010 = (ref_ci * spec.order) * ny_global * nz_global +
|
||||
((ref_cj * spec.order) + spec.order) * nz_global +
|
||||
(ref_ck * spec.order);
|
||||
int g001 = (ref_ci * spec.order) * ny_global * nz_global +
|
||||
(ref_cj * spec.order) * nz_global +
|
||||
((ref_ck * spec.order) + spec.order);
|
||||
|
||||
// edge vectors (columns of the deformation gradient F = R * S)
|
||||
// we store them as rows in R0 to use mjuu_mulvecmat for applying R0^{-1}
|
||||
double R0[9];
|
||||
for (int d = 0; d < 3; d++) {
|
||||
R0[0+d] = nodexpos[3*g100 + d] - nodexpos[3*g000 + d];
|
||||
R0[3+d] = nodexpos[3*g010 + d] - nodexpos[3*g000 + d];
|
||||
R0[6+d] = nodexpos[3*g001 + d] - nodexpos[3*g000 + d];
|
||||
}
|
||||
|
||||
// normalize to get rotation matrix columns (valid for regular grids)
|
||||
double li = mjuu_normvec(R0+0, 3);
|
||||
double lj = mjuu_normvec(R0+3, 3);
|
||||
double lk = mjuu_normvec(R0+6, 3);
|
||||
(void)li; (void)lj; (void)lk;
|
||||
|
||||
// apply inverse rotation to each nodexpos to get local-frame positions
|
||||
for (int i = 0; i < nnode; i++) {
|
||||
const double* p = nodexpos.data() + 3*i;
|
||||
double* q = nodexpos_local.data() + 3*i;
|
||||
mjuu_mulvecmat(q, p, R0);
|
||||
}
|
||||
} else {
|
||||
nodexpos_local = nodexpos;
|
||||
}
|
||||
return nodexpos_local;
|
||||
}
|
||||
|
||||
|
||||
// create flex BVH
|
||||
void mjCFlex::CreateBVH() {
|
||||
|
||||
@@ -1054,7 +1054,8 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
|
||||
std::vector<double> vert0_; // vertex positions in [0, 1]^d in the bounding box
|
||||
std::vector<double> node0_; // node Cartesian positions
|
||||
|
||||
|
||||
// compute unrotated node positions for stiffness computation
|
||||
std::vector<double> ComputeUnrotatedNodePositions(const std::vector<double>& nodexpos) const;
|
||||
|
||||
// stiffness caching
|
||||
std::string ComputeStiffnessCacheKey() const;
|
||||
|
||||
@@ -15,8 +15,6 @@
|
||||
// Tests for engine/engine_core_constraint.c.
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@@ -914,5 +912,143 @@ TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
|
||||
}
|
||||
}
|
||||
|
||||
// --------------------------- strain constraint rotated parent ----------------
|
||||
|
||||
struct StrainConstraintTestCase {
|
||||
std::string test_name;
|
||||
std::string body_pos;
|
||||
std::string body_quat;
|
||||
std::string flex_spacing;
|
||||
std::string flex_xyaxes;
|
||||
};
|
||||
|
||||
class StrainConstraintRotatedTest : public CoreConstraintTest,
|
||||
public ::testing::WithParamInterface<
|
||||
StrainConstraintTestCase> {
|
||||
};
|
||||
|
||||
TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
|
||||
auto param = GetParam();
|
||||
std::string xml = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast" jacobian="dense" gravity="0 0 0"/>
|
||||
<worldbody>
|
||||
<body name="parent" )";
|
||||
|
||||
if (!param.body_pos.empty()) {
|
||||
xml += "pos=\"" + param.body_pos + "\" ";
|
||||
}
|
||||
if (!param.body_quat.empty()) {
|
||||
xml += "quat=\"" + param.body_quat + "\" ";
|
||||
}
|
||||
xml += R"(>
|
||||
<joint type="free"/>
|
||||
<geom type="box" size=".01 .01 .01" mass=".1"/>
|
||||
<flexcomp name="test" type="box" )";
|
||||
|
||||
if (!param.flex_spacing.empty()) {
|
||||
xml += "spacing=\"" + param.flex_spacing + "\" ";
|
||||
}
|
||||
if (!param.flex_xyaxes.empty()) {
|
||||
xml += "xyaxes=\"" + param.flex_xyaxes + "\" ";
|
||||
}
|
||||
|
||||
xml += R"(radius="0.001"
|
||||
pos="0 0 0" dof="trilinear" mass="1" dim="3">
|
||||
<contact selfcollide="none"/>
|
||||
<edge equality="strain"/>
|
||||
</flexcomp>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
std::array<char, 1024> error;
|
||||
mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size());
|
||||
ASSERT_THAT(m, NotNull()) << error.data();
|
||||
mjData* d = mj_makeData(m);
|
||||
|
||||
mj_forward(m, d);
|
||||
|
||||
// Check we have strain constraints
|
||||
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
|
||||
|
||||
// The critical check: constraint residuals must be ~0 at the initial
|
||||
// (undeformed) configuration, even though the body is rotated.
|
||||
mjtNum max_pos = 0;
|
||||
for (int i = 0; i < d->ne; i++) {
|
||||
max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i]));
|
||||
}
|
||||
EXPECT_LT(max_pos, 1e-6)
|
||||
<< "Strain constraint residual should be ~0"
|
||||
<< " (max_pos=" << max_pos << ")";
|
||||
|
||||
// Verify stability
|
||||
for (int i = 0; i < 200; i++) {
|
||||
mj_step(m, d);
|
||||
ASSERT_FALSE(mju_isBad(d->qpos[0]))
|
||||
<< "Simulation unstable at step " << i;
|
||||
for (int j = 0; j < m->nv; j++) {
|
||||
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
|
||||
<< "Velocity exploded at step " << i
|
||||
<< ", qvel[" << j << "]=" << d->qvel[j];
|
||||
}
|
||||
}
|
||||
|
||||
mj_deleteData(d);
|
||||
mj_deleteModel(m);
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
StrainConstraintRotatedTests, StrainConstraintRotatedTest,
|
||||
testing::ValuesIn<StrainConstraintTestCase>({
|
||||
// Test strain constraint with a rotated parent body.
|
||||
// The flexcomp is placed inside a parent body that has a non-identity
|
||||
// initial rotation. This reproduces the "grocery scene" bug where the
|
||||
// stiffness matrix eigenvectors and reference positions were computed
|
||||
// in world frame instead of the unrotated local frame, causing
|
||||
// spurious constraint forces.
|
||||
{
|
||||
"RotatedParent",
|
||||
"1 2 3",
|
||||
"0.707107 0 0.707107 0",
|
||||
".1 .1 .1",
|
||||
""
|
||||
},
|
||||
// Same test with an anisotropic box (different spacing per axis) and
|
||||
// arbitrary rotation (combined 45-deg Y + 30-deg X).
|
||||
{
|
||||
"RotatedParentAnisotropic",
|
||||
"0.5 -1 2",
|
||||
"0.8924 0.2392 0.3696 -0.0990",
|
||||
".15 .08 .05",
|
||||
""
|
||||
},
|
||||
// Test strain constraint with flexcomp-level xyaxes rotation.
|
||||
// This is the "grocery scene" pattern where the flexcomp grid itself is
|
||||
// rotated via xyaxes="0 1 0 0 0 1" (X->Y, Y->Z).
|
||||
{
|
||||
"FlexcompXyaxes",
|
||||
"",
|
||||
"",
|
||||
".1 .02 .1",
|
||||
"0 1 0 0 0 1"
|
||||
},
|
||||
// Test combining parent body rotation with flexcomp xyaxes rotation.
|
||||
// The total rotation is the composition of both.
|
||||
{
|
||||
"RotatedParentPlusXyaxes",
|
||||
"1 2 3",
|
||||
"0.707107 0 0.707107 0",
|
||||
".15 .08 .05",
|
||||
"0 1 0 0 0 1"
|
||||
}
|
||||
}),
|
||||
[](const testing::TestParamInfo<
|
||||
StrainConstraintRotatedTest::ParamType>& info) {
|
||||
return info.param.test_name;
|
||||
}
|
||||
);
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
Reference in New Issue
Block a user