Introduce trilinear flex parametrization.
These flexes use only 24 DOFs (3 per vertex of the bounding box), while colliding with the full high resolution mesh. On an 8x8x8 cube, the performance using DOFs at all vertices is ``` Simulation time : 18.74 s Steps per second : 533 Realtime factor : 0.53 x Time per step : 1874.4 µs Contacts per step : 114.88 Constraints per step : 3322.51 Degrees of freedom : 1536 ``` With the new implementation, it is the following: ``` Simulation time : 1.82 s Steps per second : 5507 Realtime factor : 5.51 x Time per step : 181.6 µs Contacts per step : 38.84 Constraints per step : 155.36 Degrees of freedom : 24 ``` PiperOrigin-RevId: 721008829 Change-Id: I833df027527db578d86667cc4b24295bcf6f7d22
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
1a4b821b6b
commit
7cdf180641
+237
-6
@@ -13,6 +13,7 @@
|
||||
// limitations under the License.
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <climits>
|
||||
#include <cmath>
|
||||
#include <csetjmp>
|
||||
@@ -2402,7 +2403,7 @@ void mjCSkin::LoadSKN(mjResource* resource) {
|
||||
|
||||
|
||||
|
||||
//--------------------- elasticity implementation --------------------------------------------------
|
||||
//-------------------------- nonlinear elasticity --------------------------------------------------
|
||||
|
||||
// hash function for std::pair
|
||||
struct PairHash
|
||||
@@ -2628,6 +2629,153 @@ void inline ComputeStiffness(std::vector<double>& stiffness,
|
||||
MetricTensor<T>(stiffness.data(), t, mu, la, basis);
|
||||
}
|
||||
|
||||
//----------------------------- linear elasticity --------------------------------------------------
|
||||
|
||||
// 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.");
|
||||
|
||||
// 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;
|
||||
} else {
|
||||
return s;
|
||||
}
|
||||
}
|
||||
|
||||
// evaluate gradient fo 1-dimensional basis function
|
||||
double dphi(const double s, const double component) {
|
||||
if (component == 0) {
|
||||
return -1;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
typedef std::array<std::array<double, 3>, 3> Matrix;
|
||||
|
||||
// symmetrize a tensor
|
||||
Matrix inline sym(const Matrix& tensor) {
|
||||
Matrix eps;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
eps[i][j] = (tensor[i][j] + tensor[j][i]) / 2;
|
||||
}
|
||||
}
|
||||
return eps;
|
||||
}
|
||||
|
||||
// compute tensor inner product
|
||||
Matrix inline inner(const Matrix& tensor1, const Matrix& tensor2) {
|
||||
Matrix inner;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
inner[i][j] = tensor1[i][0] * tensor2[0][j] +
|
||||
tensor1[i][1] * tensor2[1][j] +
|
||||
tensor1[i][2] * tensor2[2][j];
|
||||
}
|
||||
}
|
||||
return inner;
|
||||
}
|
||||
|
||||
// compute trace of a tensor
|
||||
double inline trace(const Matrix& tensor) {
|
||||
return tensor[0][0] + tensor[1][1] + tensor[2][2];
|
||||
}
|
||||
|
||||
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 = std::pow(order, 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);
|
||||
|
||||
// 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 detJ = dx * dy * dz;
|
||||
double invJ[3] = {1.0 / dx, 1.0 / dy, 1.0 / dz};
|
||||
|
||||
// compute stiffness matrix
|
||||
std::vector<std::array<double, 3>> F(n);
|
||||
double la = E * nu / (1 + nu) / (1 - 2 * nu);
|
||||
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++) {
|
||||
double s = points[ps];
|
||||
double t = points[pt];
|
||||
double u = points[pu];
|
||||
double dvol = weight[ps] * weight[pt] * weight[pu] * detJ;
|
||||
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++) {
|
||||
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);
|
||||
F[dof++] = gradient;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (dof != n) { // SHOULD NOT OCCUR
|
||||
throw mjCError(NULL, "incorrect number of basis functions");
|
||||
}
|
||||
|
||||
// tensor contraction of the gradients of elastic strains
|
||||
// (d(F+F')/dx : d(F+F')/dx)
|
||||
for (int i=0; i < n; i++) {
|
||||
for (int j=0; j < n; j++) {
|
||||
Matrix du;
|
||||
Matrix dv;
|
||||
du.fill({0, 0, 0});
|
||||
dv.fill({0, 0, 0});
|
||||
for (int k=0; k < 3; k++) {
|
||||
for (int l=0; l < 3; l++) {
|
||||
du[k][0] = invJ[0] * F[i][0];
|
||||
du[k][1] = invJ[1] * F[i][1];
|
||||
du[k][2] = invJ[2] * F[i][2];
|
||||
dv[l][0] = invJ[0] * F[j][0];
|
||||
dv[l][1] = invJ[1] * F[j][1];
|
||||
dv[l][2] = invJ[2] * F[j][2];
|
||||
K[ndof*(3*i+k) + 3*j+l] -= la * trace(du) * trace(dv) * dvol;
|
||||
K[ndof*(3*i+k) + 3*j+l] -= mu * trace(inner(sym(du), sym(dv))) * dvol;
|
||||
mjuu_zerovec(du[k].data(), 3);
|
||||
mjuu_zerovec(dv[l].data(), 3);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//------------------ class mjCFlex implementation --------------------------------------------------
|
||||
|
||||
// constructor
|
||||
@@ -2641,6 +2789,7 @@ mjCFlex::mjCFlex(mjCModel* _model) {
|
||||
|
||||
// clear internal variables
|
||||
nvert = 0;
|
||||
nnode = 0;
|
||||
nedge = 0;
|
||||
nelem = 0;
|
||||
matid = -1;
|
||||
@@ -2673,13 +2822,17 @@ void mjCFlex::PointToLocal() {
|
||||
spec.name = &name;
|
||||
spec.material = &spec_material_;
|
||||
spec.vertbody = &spec_vertbody_;
|
||||
spec.nodebody = &spec_nodebody_;
|
||||
spec.vert = &spec_vert_;
|
||||
spec.node = &spec_node_;
|
||||
spec.texcoord = &spec_texcoord_;
|
||||
spec.elem = &spec_elem_;
|
||||
spec.info = &info;
|
||||
material = nullptr;
|
||||
vertbody = nullptr;
|
||||
nodebody = nullptr;
|
||||
vert = nullptr;
|
||||
node = nullptr;
|
||||
texcoord = nullptr;
|
||||
elem = nullptr;
|
||||
}
|
||||
@@ -2690,6 +2843,9 @@ void mjCFlex::NameSpace(const mjCModel* m) {
|
||||
for (auto& name : spec_vertbody_) {
|
||||
name = m->prefix + name + m->suffix;
|
||||
}
|
||||
for (auto& name : spec_nodebody_) {
|
||||
name = m->prefix + name + m->suffix;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -2699,7 +2855,9 @@ void mjCFlex::CopyFromSpec() {
|
||||
spec.info = &info;
|
||||
material_ = spec_material_;
|
||||
vertbody_ = spec_vertbody_;
|
||||
nodebody_ = spec_nodebody_;
|
||||
vert_ = spec_vert_;
|
||||
node_ = spec_node_;
|
||||
texcoord_ = spec_texcoord_;
|
||||
elem_ = spec_elem_;
|
||||
|
||||
@@ -2722,6 +2880,8 @@ void mjCFlex::DelTexcoord() {
|
||||
|
||||
|
||||
void mjCFlex::ResolveReferences(const mjCModel* m) {
|
||||
vertbodyid.clear();
|
||||
nodebodyid.clear();
|
||||
for (const auto& vertbody : vertbody_) {
|
||||
mjCBase* pbody = m->FindObject(mjOBJ_BODY, vertbody);
|
||||
if (pbody) {
|
||||
@@ -2730,12 +2890,21 @@ void mjCFlex::ResolveReferences(const mjCModel* m) {
|
||||
throw mjCError(this, "unknown body '%s' in flex", vertbody.c_str());
|
||||
}
|
||||
}
|
||||
for (const auto& nodebody : nodebody_) {
|
||||
mjCBase* pbody = m->FindObject(mjOBJ_BODY, nodebody);
|
||||
if (pbody) {
|
||||
nodebodyid.push_back(pbody->id);
|
||||
} else {
|
||||
throw mjCError(this, "unknown body '%s' in flex", nodebody.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// compiler
|
||||
void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
CopyFromSpec();
|
||||
interpolated = !nodebody_.empty();
|
||||
|
||||
// set nelem; check sizes
|
||||
if (dim<1 || dim>3) {
|
||||
@@ -2747,8 +2916,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
if (elem_.size() % (dim+1)) {
|
||||
throw mjCError(this, "elem size must be multiple of (dim+1)");
|
||||
}
|
||||
if (vertbody_.empty()) {
|
||||
throw mjCError(this, "vertbody is empty");
|
||||
if (vertbody_.empty() && !interpolated) {
|
||||
throw mjCError(this, "vertbody and nodebody are both empty");
|
||||
}
|
||||
if (vert_.size() % 3) {
|
||||
throw mjCError(this, "vert size must be a multiple of 3");
|
||||
@@ -2756,6 +2925,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
if (edgestiffness>0 && dim>1) {
|
||||
throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins");
|
||||
}
|
||||
if (interpolated && selfcollide != mjFLEXSELF_NONE) {
|
||||
throw mjCError(this, "trilinear interpolation cannot do self-collision");
|
||||
}
|
||||
if (interpolated && internal) {
|
||||
throw mjCError(this, "trilinear interpolation cannot do internal collisions");
|
||||
}
|
||||
nelem = (int)elem_.size()/(dim+1);
|
||||
|
||||
// set nvert, rigid, centered; check size
|
||||
@@ -2773,6 +2948,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
throw mjCError(this, "not enough vertices");
|
||||
}
|
||||
|
||||
// set nnode
|
||||
nnode = (int)nodebody_.size();
|
||||
if (nnode && nnode!=8) {
|
||||
throw mjCError(this, "number of nodes must be 2^dim, it is %d", "", nnode);
|
||||
}
|
||||
|
||||
// check elem vertex ids
|
||||
for (const auto& elem : elem_) {
|
||||
if (elem<0 || elem>=nvert) {
|
||||
@@ -2812,7 +2993,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
}
|
||||
|
||||
// determine rigid if not already set
|
||||
if (!rigid) {
|
||||
if (!rigid && !interpolated) {
|
||||
rigid = true;
|
||||
for (unsigned i=1; i < vertbodyid.size(); i++) {
|
||||
if (vertbodyid[i]!=vertbodyid[0]) {
|
||||
@@ -2823,7 +3004,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
}
|
||||
|
||||
// determine centered if not already set
|
||||
if (!centered) {
|
||||
if (!centered && !interpolated) {
|
||||
centered = true;
|
||||
for (const auto& vert : vert_) {
|
||||
if (vert!=0) {
|
||||
@@ -2833,6 +3014,16 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
}
|
||||
}
|
||||
|
||||
if (!centered && interpolated) {
|
||||
centered = true;
|
||||
for (const auto& node : node_) {
|
||||
if (node!=0) {
|
||||
centered = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// compute global vertex positions
|
||||
vertxpos = std::vector<double> (3*nvert);
|
||||
for (int i=0; i < nvert; i++) {
|
||||
@@ -2841,11 +3032,32 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
mjuu_copyvec(vertxpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
|
||||
|
||||
// add vertex offset within body if not centered
|
||||
if (!centered) {
|
||||
if (!centered || interpolated) {
|
||||
double offset[3];
|
||||
mjuu_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0);
|
||||
mjuu_addtovec(vertxpos.data()+3*i, offset, 3);
|
||||
}
|
||||
|
||||
if (interpolated) {
|
||||
// this should happen in ResolveReferences but we need a body id in this loop to compute
|
||||
// the global vertex position, this is a hack since it is the id of the parent body
|
||||
vertbodyid[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
// compute global node positions
|
||||
std::vector<double> nodexpos = std::vector<double> (3*nnode);
|
||||
for (int i=0; i < nnode; i++) {
|
||||
// get body id, set nodexpos = body.xpos0
|
||||
int b = nodebodyid[i];
|
||||
mjuu_copyvec(nodexpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
|
||||
|
||||
// add node offset within body if not centered
|
||||
if (!centered) {
|
||||
double offset[3];
|
||||
mjuu_rotVecQuat(offset, node_.data()+3*i, model->Bodies()[b]->xquat0);
|
||||
mjuu_addtovec(nodexpos.data()+3*i, offset, 3);
|
||||
}
|
||||
}
|
||||
|
||||
// reorder tetrahedra so right-handed face orientation is outside
|
||||
@@ -2909,7 +3121,17 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
throw mjCError(this, "Poisson ratio must be in [0, 0.5)");
|
||||
}
|
||||
stiffness.assign(21*nelem, 0);
|
||||
if (interpolated) {
|
||||
int min_size = ceil(nodexpos.size()*nodexpos.size() / 21);
|
||||
if (min_size > nelem) {
|
||||
throw mjCError(this, "Trilinear dofs are require at least %d elements", "", min_size);
|
||||
}
|
||||
ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson);
|
||||
}
|
||||
for (unsigned int t = 0; t < nelem; t++) {
|
||||
if (interpolated) {
|
||||
continue;
|
||||
}
|
||||
if (dim==2) {
|
||||
ComputeStiffness<Stencil2D>(stiffness, vertxpos,
|
||||
elem_.data() + (dim + 1) * t, t, young,
|
||||
@@ -2928,6 +3150,9 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
useredge = VectorToString(edgeidx_);
|
||||
|
||||
for (const auto& vbodyid : vertbodyid) {
|
||||
if (vbodyid < 0) {
|
||||
continue;
|
||||
}
|
||||
if (model->Bodies()[vbodyid]->plugin.element) {
|
||||
mjCPlugin* plugin_instance =
|
||||
static_cast<mjCPlugin*>(model->Bodies()[vbodyid]->plugin.element);
|
||||
@@ -2954,6 +3179,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
|
||||
vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / size + 0.5;
|
||||
}
|
||||
}
|
||||
|
||||
// store node cartesian positions
|
||||
node0_.assign(3*nnode, 0);
|
||||
for (int i=0; i < nnode; i++) {
|
||||
mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user