Add shell plugin.
- Add bending passive force - Add two examples (plate and poncho) - Add stretching constraints for particles using tendons on mesh edges PiperOrigin-RevId: 566980584 Change-Id: Id0cd8e1084c05560f1008518a8afbd5311d563e2
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
c54aba15f9
commit
dd5a7115cf
@@ -22,6 +22,8 @@ set(MUJOCO_ELASTICITY_SRCS
|
||||
elasticity.cc
|
||||
elasticity.h
|
||||
register.cc
|
||||
shell.cc
|
||||
shell.h
|
||||
solid.cc
|
||||
solid.h
|
||||
)
|
||||
|
||||
@@ -14,12 +14,14 @@
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include "cable.h"
|
||||
#include "shell.h"
|
||||
#include "solid.h"
|
||||
|
||||
namespace mujoco::plugin::elasticity {
|
||||
|
||||
mjPLUGIN_LIB_INIT {
|
||||
Cable::RegisterPlugin();
|
||||
Shell::RegisterPlugin();
|
||||
Solid::RegisterPlugin();
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,305 @@
|
||||
// Copyright 2023 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.
|
||||
|
||||
#include <algorithm>
|
||||
#include <cctype>
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstdlib>
|
||||
#include <functional>
|
||||
#include <sstream>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "shell.h"
|
||||
|
||||
|
||||
namespace mujoco::plugin::elasticity {
|
||||
namespace {
|
||||
|
||||
// local tetrahedron numbering
|
||||
constexpr int kNumEdges = Stencil2D::kNumEdges;
|
||||
constexpr int kNumVerts = Stencil2D::kNumVerts;
|
||||
constexpr int edge[kNumEdges][2] = {{1, 2}, {2, 0}, {0, 1}};
|
||||
|
||||
// copied from void mjXUtil
|
||||
void String2Vector(const std::string& txt, std::vector<int>& vec) {
|
||||
std::stringstream strm(txt);
|
||||
vec.clear();
|
||||
|
||||
while (!strm.eof()) {
|
||||
int num;
|
||||
strm >> num;
|
||||
if (strm.fail()) {
|
||||
break;
|
||||
} else {
|
||||
vec.push_back(num);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// cotangent between two edges
|
||||
mjtNum cot(mjtNum* x, int v0, int v1, int v2) {
|
||||
mjtNum normal[3];
|
||||
mjtNum edge1[3];
|
||||
mjtNum edge2[3];
|
||||
|
||||
mju_sub3(edge1, x+3*v1, x+3*v0);
|
||||
mju_sub3(edge2, x+3*v2, x+3*v0);
|
||||
mju_cross(normal, edge1, edge2);
|
||||
|
||||
return mju_dot3(edge1, edge2) / mju_norm3(normal);
|
||||
}
|
||||
|
||||
// area of a triangle
|
||||
mjtNum ComputeVolume(const mjtNum* x, const int v[kNumVerts]) {
|
||||
mjtNum normal[3];
|
||||
mjtNum edge1[3];
|
||||
mjtNum edge2[3];
|
||||
|
||||
mju_sub3(edge1, x+3*v[1], x+3*v[0]);
|
||||
mju_sub3(edge2, x+3*v[2], x+3*v[0]);
|
||||
mju_cross(normal, edge1, edge2);
|
||||
|
||||
return mju_norm3(normal) / 2;
|
||||
}
|
||||
|
||||
// reads numeric attributes
|
||||
bool CheckAttr(const char* name, const mjModel* m, int instance) {
|
||||
char* end;
|
||||
std::string value = mj_getPluginConfig(m, instance, name);
|
||||
value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end());
|
||||
strtod(value.c_str(), &end);
|
||||
return end == value.data() + value.size();
|
||||
}
|
||||
|
||||
struct PairHash
|
||||
{
|
||||
template <class T1, class T2>
|
||||
std::size_t operator() (const std::pair<T1, T2>& pair) const {
|
||||
return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
// factory function
|
||||
std::optional<Shell> Shell::Create(const mjModel* m, mjData* d, int instance) {
|
||||
if (CheckAttr("face", m, instance) &&
|
||||
CheckAttr("poisson", m, instance) &&
|
||||
CheckAttr("young", m, instance) &&
|
||||
CheckAttr("thickness", m, instance)) {
|
||||
mjtNum nu = strtod(mj_getPluginConfig(m, instance, "poisson"), nullptr);
|
||||
mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr);
|
||||
mjtNum thick =
|
||||
strtod(mj_getPluginConfig(m, instance, "thickness"), nullptr);
|
||||
mjtNum damp =
|
||||
strtod(mj_getPluginConfig(m, instance, "damping"), nullptr);
|
||||
std::vector<int> face;
|
||||
String2Vector(mj_getPluginConfig(m, instance, "face"), face);
|
||||
return Shell(m, d, instance, nu, E, damp, thick, face);
|
||||
} else {
|
||||
mju_warning("Invalid parameter specification in shell plugin");
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
// create map from triangles to vertices and edges and from edges to vertices
|
||||
void Shell::CreateStencils(const std::vector<int>& face) {
|
||||
// populate stencil
|
||||
nt = face.size() / kNumVerts;
|
||||
elements.resize(nt);
|
||||
for (int t = 0; t < nt; t++) {
|
||||
for (int v = 0; v < kNumVerts; v++) {
|
||||
elements[t].vertices[v] = face[kNumVerts*t+v]-1;
|
||||
}
|
||||
}
|
||||
|
||||
// map from edge vertices to their index in `edges` vector
|
||||
std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices;
|
||||
|
||||
// loop over all triangles
|
||||
for (int t = 0; t < nt; t++) {
|
||||
int* v = elements[t].vertices;
|
||||
|
||||
// compute edges to vertices map for fast computations
|
||||
for (int e = 0; e < kNumEdges; e++) {
|
||||
auto pair = std::pair(
|
||||
std::min(v[edge[e][0]], v[edge[e][1]]),
|
||||
std::max(v[edge[e][0]], v[edge[e][1]])
|
||||
);
|
||||
|
||||
// if edge is already present in the vector only store its index
|
||||
auto [it, inserted] = edge_indices.insert({pair, ne});
|
||||
|
||||
if (inserted) {
|
||||
StencilFlap flap;
|
||||
flap.vertices[0] = v[edge[e][0]];
|
||||
flap.vertices[1] = v[edge[e][1]];
|
||||
flap.vertices[2] = v[(edge[e][1]+1) % 3];
|
||||
flap.vertices[3] = -1;
|
||||
flaps.push_back(flap);
|
||||
elements[t].edges[e] = ne++;
|
||||
} else {
|
||||
elements[t].edges[e] = it->second;
|
||||
flaps[it->second].vertices[3] = v[(edge[e][1]+1) % 3];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// plugin constructor
|
||||
Shell::Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
|
||||
mjtNum damp, mjtNum thick, const std::vector<int>& face)
|
||||
: damping(damp), thickness(thick) {
|
||||
// count plugin bodies
|
||||
nv = ne = 0;
|
||||
for (int i = 1; i < m->nbody; i++) {
|
||||
if (m->body_plugin[i] == instance) {
|
||||
if (!nv++) {
|
||||
i0 = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// generate triangles from the vertices
|
||||
CreateStencils(face);
|
||||
|
||||
// material parameters
|
||||
mjtNum mu = E / (2*(1+nu));
|
||||
|
||||
// loop over all triangles
|
||||
for (int t = 0; t < nt; t++) {
|
||||
int* v = elements[t].vertices;
|
||||
for (int i = 0; i < kNumVerts; i++) {
|
||||
if (m->body_plugin[i0+v[i]] != instance) {
|
||||
mju_error("This body does not have the requested plugin instance");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// allocate array
|
||||
position.assign(nv*3, 0);
|
||||
bending.assign(ne*16, 0);
|
||||
|
||||
// store previous positions
|
||||
mju_copy(position.data(), m->body_pos+3*i0, 3*nv);
|
||||
|
||||
// assemble bending Hessian
|
||||
for (int e = 0; e < ne; e++) {
|
||||
int* v = flaps[e].vertices;
|
||||
int vadj[3] = {v[1], v[0], v[3]};
|
||||
|
||||
if (v[3]== -1) {
|
||||
// skip boundary edges
|
||||
continue;
|
||||
}
|
||||
|
||||
// cotangent operator from Wardetzky at al., "Discrete Quadratic Curvature
|
||||
// Energies", https://cims.nyu.edu/gcl/papers/wardetzky2007dqb.pdf
|
||||
|
||||
mjtNum a01 = cot(m->body_pos+3*i0, v[0], v[1], v[2]);
|
||||
mjtNum a02 = cot(m->body_pos+3*i0, v[0], v[3], v[1]);
|
||||
mjtNum a03 = cot(m->body_pos+3*i0, v[1], v[2], v[0]);
|
||||
mjtNum a04 = cot(m->body_pos+3*i0, v[1], v[0], v[3]);
|
||||
mjtNum c[4] = {a03 + a04, a01 + a02, -(a01 + a03), -(a02 + a04)};
|
||||
mjtNum volume = ComputeVolume(m->body_pos+3*i0, v) +
|
||||
ComputeVolume(m->body_pos+3*i0, vadj);
|
||||
|
||||
for (int v1 = 0; v1 < StencilFlap::kNumVerts; v1++) {
|
||||
for (int v2 = 0; v2 < StencilFlap::kNumVerts; v2++) {
|
||||
bending[16 * e + 4 * v1 + v2] +=
|
||||
1.5 * c[v1] * c[v2] / volume * mu * pow(thickness, 3) / 12;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Shell::Compute(const mjModel* m, mjData* d, int instance) {
|
||||
mjtNum kD = damping / m->opt.timestep;
|
||||
|
||||
// compute bending contribution
|
||||
mjtNum embedding;
|
||||
for (int e = 0; e < ne; e++) {
|
||||
int* v = flaps[e].vertices;
|
||||
mjtNum force[12] = {0};
|
||||
if (v[3] == -1) {
|
||||
// skip boundary edges
|
||||
continue;
|
||||
}
|
||||
for (int i = 0; i < StencilFlap::kNumVerts; i++) {
|
||||
for (int j = 0; j < StencilFlap::kNumVerts; j++) {
|
||||
for (int x = 0; x < 3; x++) {
|
||||
embedding = d->xpos[3*(i0+v[j])+x]*(kD+1) - position[3*v[j]+x]*kD;
|
||||
force[3*i+x] += bending[16*e+4*i+j] * embedding;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update stored positions
|
||||
mju_copy(position.data(), d->xpos+3*i0, 3*nv);
|
||||
|
||||
// insert into global force
|
||||
for (int i = 0; i < StencilFlap::kNumVerts; i++) {
|
||||
for (int x = 0; x < 3; x++) {
|
||||
d->qfrc_passive[m->body_dofadr[i0]+3*v[i]+x] -= force[3*i+x];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Shell::RegisterPlugin() {
|
||||
mjpPlugin plugin;
|
||||
mjp_defaultPlugin(&plugin);
|
||||
|
||||
plugin.name = "mujoco.elasticity.shell";
|
||||
plugin.capabilityflags |= mjPLUGIN_PASSIVE;
|
||||
|
||||
const char* attributes[] = {"face", "young", "poisson", "damping",
|
||||
"thickness"};
|
||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
||||
plugin.attributes = attributes;
|
||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||
|
||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||
auto elasticity_or_null = Shell::Create(m, d, instance);
|
||||
if (!elasticity_or_null.has_value()) {
|
||||
return -1;
|
||||
}
|
||||
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
|
||||
new Shell(std::move(*elasticity_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Shell*>(d->plugin_data[instance]);
|
||||
d->plugin_data[instance] = 0;
|
||||
};
|
||||
plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) {
|
||||
auto* elasticity = reinterpret_cast<Shell*>(d->plugin_data[instance]);
|
||||
elasticity->Compute(m, d, instance);
|
||||
};
|
||||
|
||||
mjp_registerPlugin(&plugin);
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::elasticity
|
||||
@@ -0,0 +1,79 @@
|
||||
// Copyright 2023 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.
|
||||
|
||||
#ifndef MUJOCO_PLUGIN_ELASTICITY_SHELL_H_
|
||||
#define MUJOCO_PLUGIN_ELASTICITY_SHELL_H_
|
||||
|
||||
#include <optional>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
|
||||
|
||||
namespace mujoco::plugin::elasticity {
|
||||
|
||||
struct Stencil2D {
|
||||
static constexpr int kNumEdges = 3;
|
||||
static constexpr int kNumVerts = 3;
|
||||
int vertices[kNumVerts];
|
||||
int edges[kNumEdges];
|
||||
};
|
||||
|
||||
struct StencilFlap {
|
||||
static constexpr int kNumVerts = 4;
|
||||
int vertices[kNumVerts];
|
||||
};
|
||||
|
||||
class Shell {
|
||||
public:
|
||||
// Returns a new Shell instance or nullopt on failure.
|
||||
static std::optional<Shell> Create(const mjModel* m, mjData* d,
|
||||
int instance);
|
||||
Shell(Shell&&) = default;
|
||||
|
||||
Shell& operator=(Shell&& other) = default;
|
||||
|
||||
void Compute(const mjModel* m, mjData* d, int instance);
|
||||
|
||||
static void RegisterPlugin();
|
||||
|
||||
int i0; // index of first body
|
||||
int nc; // number of quads in the grid
|
||||
int nv; // number of vertices (bodies) in the Shell
|
||||
int nt; // number of area elements (triangles)
|
||||
int ne; // number of edges in the Shell
|
||||
|
||||
// connectivity info for mapping tetrahedra to edges and vertices
|
||||
std::vector<Stencil2D> elements; // triangles (nt x 6)
|
||||
std::vector<StencilFlap> flaps; // adjacent triangles (ne x 4)
|
||||
|
||||
// precomputed quantities
|
||||
std::vector<mjtNum> position; // previous-step positions (nv x 3)
|
||||
std::vector<mjtNum> bending; // bending Hessian (ne x 16)
|
||||
|
||||
mjtNum damping;
|
||||
mjtNum thickness;
|
||||
|
||||
private:
|
||||
Shell(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
|
||||
mjtNum damp, mjtNum thick, const std::vector<int>& face);
|
||||
|
||||
void CreateStencils(const std::vector<int>& face);
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::elasticity
|
||||
|
||||
#endif // MUJOCO_PLUGIN_ELASTICITY_SHELL_H_
|
||||
Reference in New Issue
Block a user