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
+65
-12
@@ -86,6 +86,7 @@ mjCFlexcomp::mjCFlexcomp(void) {
|
||||
mjuu_setvec(quat, 1, 0, 0, 0);
|
||||
rigid = false;
|
||||
centered = false;
|
||||
doftype = mjFCOMPDOF_FULL;
|
||||
|
||||
mjs_defaultPlugin(&plugin);
|
||||
mjs_defaultOrientation(&alt);
|
||||
@@ -102,10 +103,6 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
|
||||
mjCModel* model = static_cast<mjCBody*>(body->element)->model;
|
||||
mjsCompiler* compiler = static_cast<mjCBody*>(body->element)->compiler;
|
||||
mjsFlex* dflex = def.spec.flex;
|
||||
|
||||
bool radial = (type == mjFCOMPTYPE_BOX ||
|
||||
type == mjFCOMPTYPE_CYLINDER ||
|
||||
type == mjFCOMPTYPE_ELLIPSOID);
|
||||
bool direct = (type == mjFCOMPTYPE_DIRECT ||
|
||||
type == mjFCOMPTYPE_MESH ||
|
||||
type == mjFCOMPTYPE_GMSH);
|
||||
@@ -122,7 +119,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
|
||||
|
||||
// check counts
|
||||
for (int i=0; i < 3; i++) {
|
||||
if (count[i] < 1 || ((radial && count[i] < 2) && dflex->dim == 3)) {
|
||||
if (count[i] < 1 || ((doftype == mjFCOMPDOF_RADIAL && count[i] < 2) && dflex->dim == 3)) {
|
||||
return comperr(error, "Count too small", error_sz);
|
||||
}
|
||||
}
|
||||
@@ -260,6 +257,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
|
||||
point[3*i+2] = newp[2];
|
||||
}
|
||||
|
||||
// compute bounding box of points
|
||||
double minmax[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
|
||||
for (int i=0; i < npnt; i++) {
|
||||
for (int j=0; j < 3; j++) {
|
||||
minmax[j+0] = std::min(minmax[j+0], point[3*i+j]);
|
||||
minmax[j+3] = std::max(minmax[j+3], point[3*i+j]);
|
||||
}
|
||||
}
|
||||
|
||||
// construct pinned array
|
||||
pinned = vector<bool>(npnt, rigid);
|
||||
|
||||
@@ -337,7 +343,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
|
||||
}
|
||||
|
||||
// center of radial body is always pinned
|
||||
if (radial) {
|
||||
if (doftype == mjFCOMPDOF_RADIAL) {
|
||||
pinned[0] = true;
|
||||
}
|
||||
|
||||
@@ -434,8 +440,8 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// pinned: parent body
|
||||
if (pinned[i]) {
|
||||
// pinned or trilinear: parent body
|
||||
if (pinned[i] || doftype == mjFCOMPDOF_TRILINEAR) {
|
||||
mjs_appendString(pf->vertbody, mjs_getString(body->name));
|
||||
|
||||
// add plugin
|
||||
@@ -448,7 +454,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
|
||||
}
|
||||
}
|
||||
|
||||
// not pinned: new body
|
||||
// not pinned and not trilinear: new body
|
||||
else {
|
||||
// add new body at vertex coordinates
|
||||
mjsBody* pb = mjs_addBody(body, 0);
|
||||
@@ -465,7 +471,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
|
||||
pb->explicitinertial = true;
|
||||
|
||||
// add radial slider
|
||||
if (radial) {
|
||||
if (doftype == mjFCOMPDOF_RADIAL) {
|
||||
mjsJoint* jnt = mjs_addJoint(pb, 0);
|
||||
|
||||
// set properties
|
||||
@@ -476,7 +482,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
|
||||
}
|
||||
|
||||
// add three orthogonal sliders
|
||||
else {
|
||||
else if (doftype == mjFCOMPDOF_FULL) {
|
||||
for (int j=0; j < 3; j++) {
|
||||
// add joint to body
|
||||
mjsJoint* jnt = mjs_addJoint(pb, 0);
|
||||
@@ -513,7 +519,54 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
|
||||
}
|
||||
}
|
||||
|
||||
if (!centered) {
|
||||
// create nodal mesh for trilinear interpolation
|
||||
if (doftype == mjFCOMPDOF_TRILINEAR) {
|
||||
std::vector<double> node(24, 0);
|
||||
for (int i=0; i < 2; i++) {
|
||||
for (int j=0; j < 2; j++) {
|
||||
for (int k=0; k < 2; k++) {
|
||||
if (pinned[i*4+j*2+k]) {
|
||||
node[3*(i*4+j*2+k)+0] = i == 0 ? minmax[0] : minmax[3];
|
||||
node[3*(i*4+j*2+k)+1] = j == 0 ? minmax[1] : minmax[4];
|
||||
node[3*(i*4+j*2+k)+2] = k == 0 ? minmax[2] : minmax[5];
|
||||
mjs_appendString(pf->nodebody, mjs_getString(body->name));
|
||||
continue;
|
||||
}
|
||||
|
||||
mjsBody* pb = mjs_addBody(body, 0);
|
||||
pb->pos[0] = i == 0 ? minmax[0] : minmax[3];
|
||||
pb->pos[1] = j == 0 ? minmax[1] : minmax[4];
|
||||
pb->pos[2] = k == 0 ? minmax[2] : minmax[5];
|
||||
mjuu_zerovec(pb->ipos, 3);
|
||||
pb->mass = mass / 8;
|
||||
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;
|
||||
pb->explicitinertial = true;
|
||||
|
||||
for (int d=0; d < 3; d++) {
|
||||
mjsJoint* jnt = mjs_addJoint(pb, 0);
|
||||
jnt->type = mjJNT_SLIDE;
|
||||
mjuu_setvec(jnt->pos, 0, 0, 0);
|
||||
mjuu_setvec(jnt->axis, 0, 0, 0);
|
||||
jnt->axis[d] = 1;
|
||||
}
|
||||
|
||||
// construct node name, add to nodebody
|
||||
char txt[100];
|
||||
mju::sprintf_arr(txt, "%s_%d_%d_%d", name.c_str(), i, j, k);
|
||||
mjs_setString(pb->name, txt);
|
||||
mjs_appendString(pf->nodebody, mjs_getString(pb->name));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!centered) {
|
||||
mjs_setDouble(pf->node, node.data(), node.size());
|
||||
}
|
||||
}
|
||||
|
||||
if (!centered || doftype == mjFCOMPDOF_TRILINEAR) {
|
||||
mjs_setDouble(pf->vert, point.data(), point.size());
|
||||
}
|
||||
|
||||
|
||||
@@ -40,6 +40,15 @@ typedef enum _mjtFcompType {
|
||||
} mjtFcompType;
|
||||
|
||||
|
||||
typedef enum _mjtDof {
|
||||
mjFCOMPDOF_FULL = 0,
|
||||
mjFCOMPDOF_RADIAL,
|
||||
mjFCOMPDOF_TRILINEAR,
|
||||
|
||||
mjNFCOMPDOFS
|
||||
} mjtDof;
|
||||
|
||||
|
||||
class mjCFlexcomp {
|
||||
public:
|
||||
mjCFlexcomp(void);
|
||||
@@ -73,6 +82,7 @@ class mjCFlexcomp {
|
||||
double inertiabox; // size of inertia box for each body
|
||||
bool equality; // create edge equality constraint
|
||||
std::string file; // mesh/gmsh file name
|
||||
mjtDof doftype; // dof type, all vertices or trilinear interpolation
|
||||
|
||||
// pin specifications
|
||||
std::vector<int> pinid; // ids of points to pin
|
||||
|
||||
+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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
+34
-4
@@ -823,6 +823,7 @@ void mjCModel::Clear() {
|
||||
nv = 0;
|
||||
nu = 0;
|
||||
na = 0;
|
||||
nflexnode = 0;
|
||||
nflexvert = 0;
|
||||
nflexedge = 0;
|
||||
nflexelem = 0;
|
||||
@@ -1733,6 +1734,7 @@ void mjCModel::SetSizes() {
|
||||
|
||||
// flex counts
|
||||
for (int i=0; i<nflex; i++) {
|
||||
nflexnode += flexes_[i]->nnode;
|
||||
nflexvert += flexes_[i]->nvert;
|
||||
nflexedge += flexes_[i]->nedge;
|
||||
nflexelem += flexes_[i]->nelem;
|
||||
@@ -2746,7 +2748,7 @@ int mjCModel::CountNJmom(const mjModel* m) {
|
||||
|
||||
// copy objects outside kinematic tree
|
||||
void mjCModel::CopyObjects(mjModel* m) {
|
||||
int adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr;
|
||||
int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr;
|
||||
int edge_adr, elem_adr, elemdata_adr, elemedge_adr, shelldata_adr, evpair_adr;
|
||||
int bonevert_adr, graph_adr, data_adr, bvh_adr;
|
||||
|
||||
@@ -2826,6 +2828,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
|
||||
// flexes
|
||||
vert_adr = 0;
|
||||
node_adr = 0;
|
||||
edge_adr = 0;
|
||||
elem_adr = 0;
|
||||
elemdata_adr = 0;
|
||||
@@ -2865,6 +2868,8 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
m->flex_dim[i] = pfl->dim;
|
||||
m->flex_vertadr[i] = vert_adr;
|
||||
m->flex_vertnum[i] = pfl->nvert;
|
||||
m->flex_nodeadr[i] = node_adr;
|
||||
m->flex_nodenum[i] = pfl->nnode;
|
||||
m->flex_edgeadr[i] = edge_adr;
|
||||
m->flex_edgenum[i] = pfl->nedge;
|
||||
m->flex_elemadr[i] = elem_adr;
|
||||
@@ -2924,16 +2929,27 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
}
|
||||
|
||||
// copy or set vert
|
||||
if (pfl->centered) {
|
||||
if (pfl->centered && !pfl->interpolated) {
|
||||
mjuu_zerovec(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
|
||||
}
|
||||
else {
|
||||
mjuu_copyvec(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert);
|
||||
}
|
||||
|
||||
// copy or set node
|
||||
if (pfl->centered && pfl->interpolated) {
|
||||
mjuu_zerovec(m->flex_node + 3*node_adr, 3*pfl->nnode);
|
||||
}
|
||||
else if (pfl->interpolated) {
|
||||
mjuu_copyvec(m->flex_node + 3*node_adr, pfl->node_.data(), 3*pfl->nnode);
|
||||
}
|
||||
|
||||
// copy vert0
|
||||
mjuu_copyvec(m->flex_vert0 + 3*vert_adr, pfl->vert0_.data(), 3*pfl->nvert);
|
||||
|
||||
// copy node0
|
||||
mjuu_copyvec(m->flex_node0 + 3*node_adr, pfl->node0_.data(), 3*pfl->nnode);
|
||||
|
||||
// copy or set vertbodyid
|
||||
if (pfl->rigid) {
|
||||
for (int k=0; k<pfl->nvert; k++) {
|
||||
@@ -2944,6 +2960,18 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
memcpy(m->flex_vertbodyid + vert_adr, pfl->vertbodyid.data(), pfl->nvert*sizeof(int));
|
||||
}
|
||||
|
||||
// copy or set nodebodyid
|
||||
if (pfl->rigid) {
|
||||
for (int k=0; k<pfl->nnode; k++) {
|
||||
m->flex_nodebodyid[node_adr + k] = pfl->nodebodyid[0];
|
||||
}
|
||||
} else {
|
||||
memcpy(m->flex_nodebodyid + node_adr, pfl->nodebodyid.data(), pfl->nnode*sizeof(int));
|
||||
}
|
||||
|
||||
// set interpolation type, only two types for now
|
||||
m->flex_interp[i] = pfl->interpolated;
|
||||
|
||||
// convert edge pairs to int array, set edge rigid
|
||||
for (int k=0; k<pfl->nedge; k++) {
|
||||
m->flex_edge[2*(edge_adr+k)] = pfl->edge[k].first;
|
||||
@@ -2951,8 +2979,9 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
|
||||
if (pfl->rigid) {
|
||||
m->flexedge_rigid[edge_adr+k] = 1;
|
||||
} else {
|
||||
} else if (!pfl->interpolated) {
|
||||
// check if vertex body weldids are the same
|
||||
// unsupported by trilinear interpolation
|
||||
int b1 = pfl->vertbodyid[pfl->edge[k].first];
|
||||
int b2 = pfl->vertbodyid[pfl->edge[k].second];
|
||||
m->flexedge_rigid[edge_adr+k] = (bodies_[b1]->weldid == bodies_[b2]->weldid);
|
||||
@@ -2961,6 +2990,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
||||
|
||||
// advance counters
|
||||
vert_adr += pfl->nvert;
|
||||
node_adr += pfl->nnode;
|
||||
edge_adr += pfl->nedge;
|
||||
elem_adr += pfl->nelem;
|
||||
elemdata_adr += (pfl->dim+1) * pfl->nelem;
|
||||
@@ -4317,7 +4347,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
|
||||
// create low-level model
|
||||
mj_makeModel(&m,
|
||||
nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite,
|
||||
ncam, nlight, nflex, nflexvert, nflexedge, nflexelem,
|
||||
ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem,
|
||||
nflexelemdata, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord,
|
||||
nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph,
|
||||
nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert,
|
||||
|
||||
@@ -86,6 +86,7 @@ class mjCModel_ : public mjsElement {
|
||||
int nbvh; // number of total boundary volume hierarchies
|
||||
int nbvhstatic; // number of static boundary volume hierarchies
|
||||
int nbvhdynamic; // number of dynamic boundary volume hierarchies
|
||||
int nflexnode; // number of nodes in all flexes
|
||||
int nflexvert; // number of vertices in all flexes
|
||||
int nflexedge; // number of edges in all flexes
|
||||
int nflexelem; // number of elements in all flexes
|
||||
|
||||
@@ -746,12 +746,15 @@ class mjCLight : public mjCLight_, private mjsLight {
|
||||
class mjCFlex_ : public mjCBase {
|
||||
protected:
|
||||
int nvert; // number of verices
|
||||
int nnode; // number of nodes
|
||||
int nedge; // number of edges
|
||||
int nelem; // number of elements
|
||||
int matid; // material id
|
||||
bool rigid; // all vertices attached to the same body
|
||||
bool centered; // all vertices coordinates (0,0,0)
|
||||
bool interpolated; // vertices are interpolated from nodes
|
||||
std::vector<int> vertbodyid; // vertex body ids
|
||||
std::vector<int> nodebodyid; // node body ids
|
||||
std::vector<std::pair<int, int>> edge; // edge vertex ids
|
||||
std::vector<int> shell; // shell fragment vertex ids (dim per fragment)
|
||||
std::vector<int> elemlayer; // element layer (distance from border)
|
||||
@@ -764,14 +767,18 @@ class mjCFlex_ : public mjCBase {
|
||||
|
||||
// variable-size data
|
||||
std::vector<std::string> vertbody_; // vertex body names
|
||||
std::vector<std::string> nodebody_; // node body names
|
||||
std::vector<double> vert_; // vertex positions
|
||||
std::vector<double> node_; // node positions
|
||||
std::vector<int> elem_; // element vertex ids
|
||||
std::vector<float> texcoord_; // vertex texture coordinates
|
||||
std::string material_; // name of material used for rendering
|
||||
|
||||
std::string spec_material_;
|
||||
std::vector<std::string> spec_vertbody_;
|
||||
std::vector<std::string> spec_nodebody_;
|
||||
std::vector<double> spec_vert_;
|
||||
std::vector<double> spec_node_;
|
||||
std::vector<int> spec_elem_;
|
||||
std::vector<float> spec_texcoord_;
|
||||
};
|
||||
@@ -804,6 +811,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
|
||||
const std::vector<double>& get_elemaabb() const { return elemaabb_; }
|
||||
const std::vector<int>& get_elem() const { return elem_; }
|
||||
const std::vector<float>& get_texcoord() const { return texcoord_; }
|
||||
const std::vector<std::string>& get_nodebody() const { return nodebody_; }
|
||||
|
||||
bool HasTexcoord() const; // texcoord not null
|
||||
void DelTexcoord(); // delete texcoord
|
||||
@@ -816,6 +824,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
|
||||
void CreateShellPair(void); // create shells and evpairs
|
||||
|
||||
std::vector<double> vert0_; // vertex positions in [0, 1]^d in the bounding box
|
||||
std::vector<double> node0_; // node Cartesian positions
|
||||
};
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user