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

Add quadratic stiffness to flex.

Browse Source 
PiperOrigin-RevId: 827409919
Change-Id: I3dff8ea49fb1726fec4acf5b91138d6d52c4bfba
This commit is contained in:
Alessio Quaglino
2025-11-03 03:00:18 -08:00
committed by Copybara-Service
parent d4f13b4abc
commit 3a7aa84e53
10 changed files with 161 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+13 -3
View File
@@ -3540,15 +3540,25 @@ saving the XML:
:align: right
:width: 240px
Trilinear flexes are much faster than the previous two options, and are the preferred choice if the expected
deformations can be captured by the reduced parametriation. For example, see the video on the right comparing `full
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper.xml>`__ and `trilinear
Trilinear and quadratic flexes are much faster than the previous two options, and are the preferred choice if the
expected deformations can be captured by the reduced parametriation. For example, see the video on the right
comparing `full <https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper.xml>`__ and `trilinear
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper_trilinear.xml>`__ flexes for modeling
deformable gripper pads.
Note that the choice of dof parametrization affects the deformation modes of the flex but has no effect on the
accuracy of the collision geometry, which always takes into account the high-resolution mesh of the flex.
**quadratic**
Three translational dofs per corner, edge, face, and volume of the bounding box of the flex, for a total of 81 dofs
for the entire flex, independent of the number of vertices. The positions of the vertices are updated using
quadratic interpolation over the bounding box. While this option requires more degrees of freedom than trilinear
flexes, it enables curved deformation modes, while the only modes achievable for trilinear flexes are
strech/compression and shear.
Note that a higher interpolation order generally requires a smaller time step for stability, although usually not as
large as with the "full" option and a fine mesh.
.. _body-flexcomp-type:
:at:`type`: :at-val:`[grid, box, cylinder, ellipsoid, disc, circle, mesh, gmsh, direct], "grid"`
doc/changelog.rst
+2
View File
@@ -8,6 +8,8 @@ Upcoming version (not yet released)
General
^^^^^^^^^
- Added "quadratic" option to :ref:`flexcomp/dof<body-flexcomp-dof>`. This type of fast :ref:`deformable<CDeformable>`
flex object is similar to the "trilinear" option, but it includes curved deformations.
- Raise an error if there are name collisions also during parsing.
- Increase Windows stack size to 16MB to enable models with deep nested body hierarchies.
- Added a new :ref:`mj_extractState` function that allows a subset of a state that was previously returned by
model/flex/bunny_quadratic.xml
+42
View File
@@ -0,0 +1,42 @@
<!-- Copyright 2024 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco model="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".0005" integrator="implicitfast"/>
<size memory="100M"/>
<visual>
<map stiffness="100"/>
</visual>
<worldbody>
<body>
<joint name="press" type="slide" axis="0 0 1" damping="500"/>
<geom type="box" size=".02 .2 .2" pos="0 0 .5"/>
</body>
<flexcomp type="mesh" file="bunny.obj" pos="0 0 -.01" dim="2" euler="90 0 0"
radius=".002" rgba="0 .7 .7 1" mass=".05" name="softbody" dof="quadratic">
<elasticity young="1e3" poisson="0.1" damping="0.0001" elastic2d="stretch"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
<actuator>
<position name="press" joint="press" gear="-1 0 0 0 0 0" ctrlrange="-1 1" kp="1000"/>
</actuator>
</mujoco>
model/flex/quadratic.xml
+3 -4
View File
@@ -13,12 +13,10 @@
limitations under the License.
-->
<mujoco model="Trilinear">
<mujoco model="Quadratic">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast">
<flag gravity="disable"/>
</option>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
<size memory="10M"/>
@@ -33,6 +31,7 @@
</body>
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1" dim="3"
radius=".001" rgba="0 .7 .7 1" mass="5" name="softbody" dof="quadratic">
<elasticity young="1e4" poisson="0.1" damping="0.0001"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
src/engine/engine_core_constraint.c
+5 -5
View File
@@ -863,8 +863,8 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di
else {
// get bodies and weights
int nb = 0;
int bid[64];
mjtNum bweight[64];
int bid[729]; // 729 = 27*27
mjtNum bweight[729];
for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
@@ -1121,8 +1121,8 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
tran = rot = 0;
for (int side=0; side < 2; side++) {
// get bodies and weights
int nb = 0, bid[32], vid[4], nw = 0;
mjtNum bweight[32], bw[4];
int nb = 0, bid[729], vid[4], nw = 0;
mjtNum bweight[729], bw[4];
// geom
if (con->geom[side] >= 0) {
@@ -1808,7 +1808,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int NV = 0;
if (nnz) {
// get bodies
int nb = 0, bid[64];
int nb = 0, bid[729];
for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
src/engine/engine_passive.c
+3 -3
View File
@@ -206,8 +206,8 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
}
if (m->flex_interp[f]) {
mjtNum xpos[mjMAXFLEXNODES], displ[mjMAXFLEXNODES], vel[mjMAXFLEXNODES];
mjtNum frc[mjMAXFLEXNODES], dmp[mjMAXFLEXNODES];
mjtNum xpos[3*mjMAXFLEXNODES], displ[3*mjMAXFLEXNODES], vel[3*mjMAXFLEXNODES];
mjtNum frc[3*mjMAXFLEXNODES], dmp[3*mjMAXFLEXNODES];
mjtNum com[3] = {0};
mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
@@ -242,7 +242,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// compute the Jacobian at the center of mass
mjtNum mat[9] = {0};
mjtNum p[3] = {.5, .5, .5};
mju_defGradient(mat, p, xpos, 1);
mju_defGradient(mat, p, xpos, m->flex_interp[f]);
// find rotation
mjtNum quat[4] = {1, 0, 0, 0};
src/engine/engine_vis_visualize.c
+15 -9
View File
@@ -1390,6 +1390,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjtNum xpos[mjMAXFLEXNODES];
int nstart = m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
int nnode = m->flex_interp[f]+1;
if (m->flex_centered[f]) {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
@@ -1400,34 +1401,39 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
}
}
for (int i=0; i < 2; i++) {
for (int j=0; j < 2; j++) {
for (int k=0; k < 2; k++) {
if (i == 0) {
for (int i=0; i < nnode; i++) {
for (int j=0; j < nnode; j++) {
for (int k=0; k < nnode; k++) {
int nn = nnode*nnode;
int offset = 3*(nn*(i+0) + nnode*(j+0) + k);
int offset1 = 3*(nn*(i+1) + nnode*(j+0) + k);
int offset2 = 3*(nn*(i+0) + nnode*(j+1) + k);
int offset3 = 3*(nn*(i+0) + nnode*(j+0) + (k+1));
if (i < nnode-1) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*(i+1)+2*j+k));
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
releaseGeom(&thisgeom, scn);
}
if (j == 0) {
if (j < nnode-1) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*(j+1)+k));
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
releaseGeom(&thisgeom, scn);
}
if (k == 0) {
if (k < nnode-1) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*j+(k+1)));
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3);
releaseGeom(&thisgeom, scn);
}
}
src/user/user_flexcomp.cc
+6 -1
View File
@@ -535,6 +535,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
std::vector<double> node(3*(order+1)*(order+1)*(order+1), 0);
int idx = 0;
double step = 1.0 / (double)order;
double massP2[3] = {1. / 6., 2. / 3., 1. / 6.};
for (int i=0; i <= order; i++) {
for (int j=0; j <= order; j++) {
for (int k=0; k <= order; k++) {
@@ -552,7 +553,11 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
pb->pos[1] = minmax[1] + j * step * (minmax[4] - minmax[1]);
pb->pos[2] = minmax[2] + k * step * (minmax[5] - minmax[2]);
mjuu_zerovec(pb->ipos, 3);
pb->mass = mass / 8;
if (doftype == mjFCOMPDOF_TRILINEAR) {
pb->mass = mass / 8;
} else {
pb->mass = mass * massP2[i] * massP2[j] * massP2[k];
}
pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
src/user/user_mesh.cc
+72 -37
View File
@@ -3788,33 +3788,69 @@ void inline ComputeBending(double* bending, double* pos, const int v[4], double
// Gauss Legendre quadrature points in 1 dimension on the interval [a, b]
void quadratureGaussLegendre(double* points, double* weights,
const int order, const double a, const double b) {
if (order > 2)
mju_error("Integration order > 2 not yet supported.");
if (order > 3)
mju_error("Integration order > 3 not yet supported.");
// x is on [-1, 1], p on [a, b]
double p0 = (a+b)/2.;
double dpdx = (b-a)/2;
points[0] = -dpdx/sqrt(3) + p0;
points[1] = dpdx/sqrt(3) + p0;
weights[0] = dpdx;
weights[1] = dpdx;
}
// evaluate 1-dimensional basis function
double phi(const double s, const double component) {
if (component == 0) {
return 1-s;
if (order == 2) {
points[0] = -dpdx / sqrt(3) + p0;
points[1] = dpdx / sqrt(3) + p0;
weights[0] = dpdx;
weights[1] = dpdx;
} else {
return s;
points[0] = p0;
points[1] = -dpdx / sqrt(3. / 5.) + p0;
points[2] = dpdx / sqrt(3. / 5.) + p0;
weights[0] = 8. / 9. * dpdx;
weights[1] = 5. / 9. * dpdx;
weights[2] = 5. / 9. * dpdx;
}
}
// evaluate gradient fo 1-dimensional basis function
double dphi(const double s, const double component) {
if (component == 0) {
return -1;
// evaluate 1-dimensional basis function
double phi(const double s, const int i, const int order) {
if (order == 1) {
return i == 0 ? 1 - s : s;
} else if (order == 2) {
switch (i) {
case 0:
return 2 * s * s - 3 * s + 1;
case 1:
return 4 * (s - s * s);
case 2:
return 2 * s * s - s;
default:
mjERROR("invalid index %d", i);
return 0;
}
} else {
return 1;
mju_error("Order must be 1 or 2.");
return 0;
}
}
// evaluate gradient of 1-dimensional basis function
double dphi(const double s, const int i, const int order) {
if (order == 1) {
return i == 0 ? -1 : 1;
} else if (order == 2) {
switch (i) {
case 0:
return 4 * s - 3;
case 1:
return 4 * (1 - 2 * s);
case 2:
return 4 * s - 1;
default:
mjERROR("invalid index %d, must be 0, 1, or 2", i);
return 0;
}
} else {
mju_error("Order must be 1 or 2.");
return 0;
}
}
@@ -3851,21 +3887,20 @@ double inline trace(const Matrix& tensor) {
void inline ComputeLinearStiffness(std::vector<double>& K,
const double* pos,
double E, double nu) {
// only linear elements are supported for now
int order = 2;
int n = pow(order, 3);
double E, double nu, int order) {
int nbasis = order + 1;
int n = pow(nbasis, 3);
int ndof = 3*n;
// compute quadrature points
std::vector<double> points(order); // quadrature points
std::vector<double> weight(order); // quadrature weights
quadratureGaussLegendre(points.data(), weight.data(), order, 0, 1);
std::vector<double> points(nbasis); // quadrature points
std::vector<double> weight(nbasis); // quadrature weights
quadratureGaussLegendre(points.data(), weight.data(), nbasis, 0, 1);
// compute element transformation
double dx = (pos+12)[0] - pos[0];
double dy = (pos+ 6)[1] - pos[1];
double dz = (pos+ 3)[2] - pos[2];
double dx = (pos+3*(n-1))[0] - pos[0];
double dy = (pos+3*(n-1))[1] - pos[1];
double dz = (pos+3*(n-1))[2] - pos[2];
double detJ = dx * dy * dz;
double invJ[3] = {1.0 / dx, 1.0 / dy, 1.0 / dz};
@@ -3875,9 +3910,9 @@ void inline ComputeLinearStiffness(std::vector<double>& K,
double mu = E / (2 * (1 + nu));
// loop over quadrature points
for (int ps=0; ps < order; ps++) {
for (int pt=0; pt < order; pt++) {
for (int pu=0; pu < order; pu++) {
for (int ps=0; ps < nbasis; ps++) {
for (int pt=0; pt < nbasis; pt++) {
for (int pu=0; pu < nbasis; pu++) {
double s = points[ps];
double t = points[pt];
double u = points[pu];
@@ -3885,13 +3920,13 @@ void inline ComputeLinearStiffness(std::vector<double>& K,
int dof = 0;
// cartesian product of basis functions
for (int bx=0; bx < order; bx++) {
for (int by=0; by < order; by++) {
for (int bz=0; bz < order; bz++) {
for (int bx=0; bx < nbasis; bx++) {
for (int by=0; by < nbasis; by++) {
for (int bz=0; bz < nbasis; bz++) {
std::array<double, 3> gradient;
gradient[0] = dphi(s, bx) * phi(t, by) * phi(u, bz);
gradient[1] = phi(s, bx) * dphi(t, by) * phi(u, bz);
gradient[2] = phi(s, bx) * phi(t, by) * dphi(u, bz);
gradient[0] = dphi(s, bx, order) * phi(t, by, order) * phi(u, bz, order);
gradient[1] = phi(s, bx, order) * dphi(t, by, order) * phi(u, bz, order);
gradient[2] = phi(s, bx, order) * phi(t, by, order) * dphi(u, bz, order);
F[dof++] = gradient;
}
}
@@ -4301,7 +4336,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (min_size > nelem) {
throw mjCError(this, "Trilinear dofs are require at least %d elements", "", min_size);
}
ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson);
ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson, order_);
}
// geometrically nonlinear elasticity
src/xml/xml_native_reader.cc
-3
View File
@@ -2723,9 +2723,6 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
ReadAttr(elasticity, "damping", 1, &dflex.damping, text);
ReadAttr(elasticity, "thickness", 1, &dflex.thickness, text);
MapValue(elasticity, "elastic2d", &dflex.elastic2d, elastic2d_map, 4);
if (fcomp.doftype == mjFCOMPDOF_QUADRATIC) {
throw mjXError(elasticity, "elasticity is not yet supported for quadratic flex");
}
}
// check errors