MakeParticle and elasticity plugin refactoring.
- Removed the explicit dependency on a plugin name. - Creation of particles from user-defined vertices (if a plugin is present). - Add skin support for an irregular mesh - Fixes #642 and #674. PiperOrigin-RevId: 561313771 Change-Id: I450f91f0d3b578cc18d97ed7180a0a8944b7dd41
This commit is contained in:
committed by
Copybara-Service
parent
f86e8b449f
commit
273c23038c
@@ -20,6 +20,8 @@ set(MUJOCO_ELASTICITY_SRCS
|
||||
cable.cc
|
||||
cable.h
|
||||
elasticity.cc
|
||||
elasticity.h
|
||||
register.cc
|
||||
solid.cc
|
||||
solid.h
|
||||
)
|
||||
|
||||
@@ -12,15 +12,54 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include "cable.h"
|
||||
#include "solid.h"
|
||||
#include "elasticity.h"
|
||||
#include <algorithm>
|
||||
#include <cctype>
|
||||
#include <cstdlib>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::plugin::elasticity {
|
||||
|
||||
mjPLUGIN_LIB_INIT {
|
||||
Cable::RegisterPlugin();
|
||||
Solid::RegisterPlugin();
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
|
||||
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
|
||||
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
|
||||
}
|
||||
|
||||
void UpdateSquaredLengths(std::vector<mjtNum>& len,
|
||||
const std::vector<std::pair<int, int> >& edges,
|
||||
const mjtNum* x) {
|
||||
for (int e = 0; e < len.size(); e++) {
|
||||
const mjtNum* p0 = x + 3*edges[e].first;
|
||||
const mjtNum* p1 = x + 3*edges[e].second;
|
||||
len[e] = SquaredDist3(p0, p1);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::elasticity
|
||||
|
||||
@@ -0,0 +1,50 @@
|
||||
// Copyright 2022 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_ELASTICITY_H_
|
||||
#define MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_
|
||||
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::plugin::elasticity {
|
||||
|
||||
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);
|
||||
}
|
||||
};
|
||||
|
||||
// copied from mjXUtil
|
||||
void String2Vector(const std::string& txt, std::vector<int>& vec);
|
||||
|
||||
// reads numeric attributes
|
||||
bool CheckAttr(const char* name, const mjModel* m, int instance);
|
||||
|
||||
// Cartesian distance between 3D vectors
|
||||
mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]);
|
||||
|
||||
// updates square lengths of edges
|
||||
void UpdateSquaredLengths(std::vector<mjtNum>& len,
|
||||
const std::vector<std::pair<int, int> >& edges,
|
||||
const mjtNum* x);
|
||||
|
||||
} // namespace mujoco::plugin::elasticity
|
||||
|
||||
#endif // MUJOCO_PLUGIN_ELASTICITY_ELASTICITY_H_
|
||||
@@ -0,0 +1,26 @@
|
||||
// Copyright 2022 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 <mujoco/mjplugin.h>
|
||||
#include "cable.h"
|
||||
#include "solid.h"
|
||||
|
||||
namespace mujoco::plugin::elasticity {
|
||||
|
||||
mjPLUGIN_LIB_INIT {
|
||||
Cable::RegisterPlugin();
|
||||
Solid::RegisterPlugin();
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::elasticity
|
||||
+23
-79
@@ -13,15 +13,17 @@
|
||||
// limitations under the License.
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstddef>
|
||||
#include <cstdio>
|
||||
#include <sstream>
|
||||
#include <cstdint>
|
||||
#include <cstdlib>
|
||||
#include <optional>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "elasticity.h"
|
||||
#include "solid.h"
|
||||
|
||||
|
||||
@@ -36,15 +38,6 @@ constexpr int edge[kNumEdges][2] = {{0, 1}, {1, 2}, {2, 0},
|
||||
constexpr int face[kNumVerts][3] = {{2, 1, 0}, {0, 1, 3}, {1, 2, 3}, {2, 0, 3}};
|
||||
constexpr int e2f[kNumEdges][2] = {{2, 3}, {1, 3}, {2, 1},
|
||||
{1, 0}, {0, 2}, {0, 3}};
|
||||
constexpr int cube2tets[kNumEdges][kNumVerts] = {{0, 3, 1, 7}, {0, 1, 4, 7},
|
||||
{1, 3, 2, 7}, {1, 2, 6, 7},
|
||||
{1, 5, 4, 7}, {1, 6, 5, 7}};
|
||||
|
||||
// Cartesian distance between 3D vectors
|
||||
mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
|
||||
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
|
||||
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
|
||||
}
|
||||
|
||||
// volume of a tetrahedron
|
||||
mjtNum ComputeVolume(const mjtNum* x, const int v[kNumVerts]) {
|
||||
@@ -89,17 +82,6 @@ void ComputeBasis(mjtNum basis[9], const mjtNum* x, const int v[kNumVerts],
|
||||
}
|
||||
}
|
||||
|
||||
// update edge lengths
|
||||
void UpdateSquaredLengths(std::vector<mjtNum>& len,
|
||||
const std::vector<std::pair<int, int> >& edges,
|
||||
const mjtNum* x) {
|
||||
for (int e = 0; e < len.size(); e++) {
|
||||
const mjtNum* p0 = x + 3*edges[e].first;
|
||||
const mjtNum* p1 = x + 3*edges[e].second;
|
||||
len[e] = SquaredDist3(p0, p1);
|
||||
}
|
||||
}
|
||||
|
||||
// gradients of edge lengths with respect to vertex positions
|
||||
void GradSquaredLengths(mjtNum gradient[kNumEdges][2][3],
|
||||
const mjtNum* x,
|
||||
@@ -113,40 +95,20 @@ void GradSquaredLengths(mjtNum gradient[kNumEdges][2][3],
|
||||
}
|
||||
}
|
||||
|
||||
// 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<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
|
||||
if (CheckAttr("nx", m, instance) &&
|
||||
CheckAttr("ny", m, instance) &&
|
||||
CheckAttr("nz", m, instance) &&
|
||||
if (CheckAttr("face", m, instance) &&
|
||||
CheckAttr("poisson", m, instance) &&
|
||||
CheckAttr("young", m, instance)) {
|
||||
int nx = strtod(mj_getPluginConfig(m, instance, "nx"), nullptr);
|
||||
int ny = strtod(mj_getPluginConfig(m, instance, "ny"), nullptr);
|
||||
int nz = strtod(mj_getPluginConfig(m, instance, "nz"), nullptr);
|
||||
mjtNum nu = strtod(mj_getPluginConfig(m, instance, "poisson"), nullptr);
|
||||
mjtNum E = strtod(mj_getPluginConfig(m, instance, "young"), nullptr);
|
||||
mjtNum damp =
|
||||
strtod(mj_getPluginConfig(m, instance, "damping"), nullptr);
|
||||
return Solid(m, d, instance, nx, ny, nz, nu, E, damp);
|
||||
std::vector<int> face;
|
||||
String2Vector(mj_getPluginConfig(m, instance, "face"), face);
|
||||
return Solid(m, d, instance, nu, E, damp, face);
|
||||
} else {
|
||||
mju_warning("Invalid parameter specification in solid plugin");
|
||||
return std::nullopt;
|
||||
@@ -154,30 +116,13 @@ std::optional<Solid> Solid::Create(const mjModel* m, mjData* d, int instance) {
|
||||
}
|
||||
|
||||
// create map from tetrahedra to vertices and edges and from edges to vertices
|
||||
void Solid::CreateStencils(int nx, int ny, int nz) {
|
||||
void Solid::CreateStencils(const std::vector<int>& simplex) {
|
||||
// populate stencil
|
||||
nt = simplex.size() / kNumVerts;
|
||||
elements.resize(nt);
|
||||
|
||||
// create a tetrahedral mesh by splitting a grid of hexahedral cells
|
||||
for (int ix = 0; ix < nx-1; ix++) {
|
||||
for (int iy = 0; iy < ny-1; iy++) {
|
||||
for (int iz = 0; iz < nz-1; iz++) {
|
||||
int t = 6*(nz-1)*(ny-1)*ix + 6*(nz-1)*iy + 6*iz;
|
||||
int vert[8] = {
|
||||
nz*ny*(ix+0) + nz*(iy+0) + iz+0,
|
||||
nz*ny*(ix+1) + nz*(iy+0) + iz+0,
|
||||
nz*ny*(ix+1) + nz*(iy+1) + iz+0,
|
||||
nz*ny*(ix+0) + nz*(iy+1) + iz+0,
|
||||
nz*ny*(ix+0) + nz*(iy+0) + iz+1,
|
||||
nz*ny*(ix+1) + nz*(iy+0) + iz+1,
|
||||
nz*ny*(ix+1) + nz*(iy+1) + iz+1,
|
||||
nz*ny*(ix+0) + nz*(iy+1) + iz+1,
|
||||
};
|
||||
for (int s = 0; s < 6; s++) {
|
||||
for (int v = 0; v < kNumVerts; v++) {
|
||||
elements[t+s].vertices[v] = vert[cube2tets[s][v]];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int t = 0; t < nt; t++) {
|
||||
for (int v = 0; v < kNumVerts; v++) {
|
||||
elements[t].vertices[v] = simplex[kNumVerts*t+v]-1;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -209,8 +154,9 @@ void Solid::CreateStencils(int nx, int ny, int nz) {
|
||||
}
|
||||
|
||||
// plugin constructor
|
||||
Solid::Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
|
||||
mjtNum nu, mjtNum E, mjtNum damp): damping(damp) {
|
||||
Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
|
||||
mjtNum damp, const std::vector<int>& simplex)
|
||||
: damping(damp) {
|
||||
// count plugin bodies
|
||||
nv = ne = 0;
|
||||
for (int i = 1; i < m->nbody; i++) {
|
||||
@@ -221,13 +167,11 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
|
||||
}
|
||||
}
|
||||
|
||||
// allocate arrays
|
||||
nc = (nx-1)*(ny-1)*(nz-1); // number of cubes
|
||||
nt = 6*nc; // number of tets
|
||||
metric.assign(kNumEdges*kNumEdges*nt, 0); // metric induced by the geometry
|
||||
|
||||
// generate tetrahedra from the vertices
|
||||
CreateStencils(nx, ny, nz);
|
||||
CreateStencils(simplex);
|
||||
|
||||
// allocate arrays
|
||||
metric.assign(kNumEdges*kNumEdges*nt, 0);
|
||||
|
||||
// loop over all tetrahedra
|
||||
for (int t = 0; t < nt; t++) {
|
||||
@@ -357,7 +301,7 @@ void Solid::RegisterPlugin() {
|
||||
plugin.name = "mujoco.elasticity.solid";
|
||||
plugin.capabilityflags |= mjPLUGIN_PASSIVE;
|
||||
|
||||
const char* attributes[] = {"nx", "ny", "nz", "young", "poisson", "damping"};
|
||||
const char* attributes[] = {"face", "young", "poisson", "damping"};
|
||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
||||
plugin.attributes = attributes;
|
||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||
|
||||
@@ -63,10 +63,10 @@ class Solid {
|
||||
mjtNum damping;
|
||||
|
||||
private:
|
||||
Solid(const mjModel* m, mjData* d, int instance, int nx, int ny, int nz,
|
||||
mjtNum nu, mjtNum E, mjtNum damp);
|
||||
Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E,
|
||||
mjtNum damp, const std::vector<int>& simplex);
|
||||
|
||||
void CreateStencils(int nx, int ny, int nz);
|
||||
void CreateStencils(const std::vector<int>& simplex);
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::elasticity
|
||||
|
||||
+202
-51
@@ -34,6 +34,7 @@
|
||||
#include "user/user_model.h"
|
||||
#include "user/user_objects.h"
|
||||
#include "user/user_util.h"
|
||||
#include "xml/xml_util.h"
|
||||
|
||||
namespace {
|
||||
namespace mju = ::mujoco::util;
|
||||
@@ -314,66 +315,178 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
|
||||
|
||||
|
||||
|
||||
// make particles
|
||||
bool mjCComposite::MakeParticle(mjCModel* model, mjCBody* body, char* error, int error_sz) {
|
||||
// create bodies and geoms
|
||||
for (int ix=0; ix<count[0]; ix++) {
|
||||
for (int iy=0; iy<count[1]; iy++) {
|
||||
for (int iz=0; iz<count[2]; iz++) {
|
||||
// create body
|
||||
char txt[100];
|
||||
mjCBody* b = body->AddBody(NULL);
|
||||
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
|
||||
b->name = txt;
|
||||
char txt[100];
|
||||
std::vector<int> face;
|
||||
|
||||
// set body position
|
||||
b->pos[0] = offset[0] + spacing*(ix - 0.5*count[0]);
|
||||
b->pos[1] = offset[1] + spacing*(iy - 0.5*count[1]);
|
||||
b->pos[2] = offset[2] + spacing*(iz - 0.5*count[2]);
|
||||
// populate vertices and names
|
||||
if (uservert.empty()) {
|
||||
if (spacing < mju_max(def[0].geom.size[0],
|
||||
mju_max(def[0].geom.size[1], def[0].geom.size[2])))
|
||||
return comperr(error, "Spacing must be larger than geometry size", error_sz);
|
||||
|
||||
// add slider joints if none defined
|
||||
if (!add[mjCOMPKIND_PARTICLE]) {
|
||||
for (int i=0; i<3; i++) {
|
||||
mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false);
|
||||
jnt->def = body->def;
|
||||
jnt->type = mjJNT_SLIDE;
|
||||
mjuu_setvec(jnt->pos, 0, 0, 0);
|
||||
mjuu_setvec(jnt->axis, 0, 0, 0);
|
||||
jnt->axis[i] = 1;
|
||||
}
|
||||
}
|
||||
for (int ix=0; ix<count[0]; ix++) {
|
||||
for (int iy=0; iy<count[1]; iy++) {
|
||||
for (int iz=0; iz<count[2]; iz++) {
|
||||
uservert.push_back(spacing*(ix - 0.5*count[0]));
|
||||
uservert.push_back(spacing*(iy - 0.5*count[1]));
|
||||
uservert.push_back(spacing*(iz - 0.5*count[2]));
|
||||
|
||||
// add user-specified joints
|
||||
else {
|
||||
for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) {
|
||||
mjCJoint* jnt = b->AddJoint(&defjnt, false);
|
||||
jnt->def = body->def;
|
||||
}
|
||||
}
|
||||
|
||||
// add geom
|
||||
mjCGeom* g = b->AddGeom(def);
|
||||
g->def = body->def;
|
||||
|
||||
// add plugin
|
||||
if (plugin_instance) {
|
||||
b->is_plugin = true;
|
||||
b->plugin_name = plugin_name;
|
||||
b->plugin_instance = plugin_instance;
|
||||
b->plugin_instance_name = plugin_instance_name;
|
||||
|
||||
// propagate attributes
|
||||
if (plugin_name == "mujoco.elasticity.solid") {
|
||||
b->plugin_instance->config_attribs["nx"] = std::to_string(count[0]);
|
||||
b->plugin_instance->config_attribs["ny"] = std::to_string(count[1]);
|
||||
b->plugin_instance->config_attribs["nz"] = std::to_string(count[2]);
|
||||
}
|
||||
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
|
||||
username.push_back(std::string(txt));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// skin
|
||||
// create faces
|
||||
if (userface.empty()) {
|
||||
if (dim == 3) {
|
||||
int cube2tets[6][4] = {{0, 3, 1, 7}, {0, 1, 4, 7},
|
||||
{1, 3, 2, 7}, {1, 2, 6, 7},
|
||||
{1, 5, 4, 7}, {1, 6, 5, 7}};
|
||||
for (int ix = 0; ix < count[0]-1; ix++) {
|
||||
for (int iy = 0; iy < count[1]-1; iy++) {
|
||||
for (int iz = 0; iz < count[2]-1; iz++) {
|
||||
int vert[8] = {
|
||||
count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+0,
|
||||
count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+0,
|
||||
count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+0,
|
||||
count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+0,
|
||||
count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+1,
|
||||
count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+1,
|
||||
count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+1,
|
||||
count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+1,
|
||||
};
|
||||
for (int s = 0; s < 6; s++) {
|
||||
for (int v = 0; v < 4; v++) {
|
||||
face.push_back(vert[cube2tets[s][v]]+1);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (dim == 2) {
|
||||
int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}};
|
||||
for (int ix = 0; ix < count[0]-1; ix++) {
|
||||
for (int iy = 0; iy < count[1]-1; iy++) {
|
||||
int vert[4] = {
|
||||
count[2]*count[1]*(ix+0) + count[2]*(iy+0),
|
||||
count[2]*count[1]*(ix+1) + count[2]*(iy+0),
|
||||
count[2]*count[1]*(ix+1) + count[2]*(iy+1),
|
||||
count[2]*count[1]*(ix+0) + count[2]*(iy+1),
|
||||
};
|
||||
for (int s = 0; s < 2; s++) {
|
||||
for (int v = 0; v < 3; v++) {
|
||||
face.push_back(vert[quad2tri[s][v]]+1);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
mjXUtil::Vector2String(userface, face);
|
||||
} else {
|
||||
dim = 2; // can only load a surface for now
|
||||
mjXUtil::String2Vector(userface, face);
|
||||
}
|
||||
|
||||
// compute volume
|
||||
std::vector<mjtNum> volume(uservert.size()/3);
|
||||
mjtNum t = 1;
|
||||
if (dim == 2 && plugin_instance) {
|
||||
// do nothing for now (until new passive forces are supported)
|
||||
}
|
||||
if (!userface.empty()) {
|
||||
mjXUtil::String2Vector(userface, face);
|
||||
for (int j=0; j<face.size()/3; j++) {
|
||||
mjtNum area[3];
|
||||
mjtNum edge1[3];
|
||||
mjtNum edge2[3];
|
||||
|
||||
for (int i=0; i<3; i++) {
|
||||
edge1[i] = uservert[3*(face[3*j+1]-1)+i] - uservert[3*(face[3*j]-1)+i];
|
||||
edge2[i] = uservert[3*(face[3*j+2]-1)+i] - uservert[3*(face[3*j]-1)+i];
|
||||
}
|
||||
|
||||
mjuu_crossvec(area, edge1, edge2);
|
||||
for (int i=0; i<3; i++) {
|
||||
volume[face[3*j+i]-1] += sqrt(mjuu_dot3(area, area)) / 2 * t;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (int i=0; i<uservert.size()/3; i++) {
|
||||
volume[i] = 6 * spacing * spacing / 2 * t;
|
||||
}
|
||||
}
|
||||
|
||||
// create bodies and geoms
|
||||
for (int i=0; i<uservert.size()/3; i++) {
|
||||
// create body
|
||||
mjCBody* b = body->AddBody(NULL);
|
||||
|
||||
if (!username.empty()) {
|
||||
b->name = username[i];
|
||||
} else {
|
||||
mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
|
||||
b->name = txt;
|
||||
}
|
||||
|
||||
// set body position
|
||||
b->pos[0] = offset[0] + uservert[3*i];
|
||||
b->pos[1] = offset[1] + uservert[3*i+1];
|
||||
b->pos[2] = offset[2] + uservert[3*i+2];
|
||||
|
||||
// add slider joints if none defined
|
||||
if (!add[mjCOMPKIND_PARTICLE]) {
|
||||
for (int i=0; i<3; i++) {
|
||||
mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false);
|
||||
jnt->def = body->def;
|
||||
jnt->type = mjJNT_SLIDE;
|
||||
mjuu_setvec(jnt->pos, 0, 0, 0);
|
||||
mjuu_setvec(jnt->axis, 0, 0, 0);
|
||||
jnt->axis[i] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// add user-specified joints
|
||||
else {
|
||||
for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) {
|
||||
mjCJoint* jnt = b->AddJoint(&defjnt, false);
|
||||
jnt->def = body->def;
|
||||
}
|
||||
}
|
||||
|
||||
// add geom
|
||||
mjCGeom* g = b->AddGeom(def);
|
||||
g->def = body->def;
|
||||
|
||||
// add site
|
||||
mjCSite* s = b->AddSite(def);
|
||||
s->def = body->def;
|
||||
s->type = mjGEOM_SPHERE;
|
||||
mju::sprintf_arr(txt, "%sS%d", prefix.c_str(), i);
|
||||
s->name = txt;
|
||||
|
||||
// add plugin
|
||||
if (plugin_instance) {
|
||||
b->is_plugin = true;
|
||||
b->plugin_name = plugin_name;
|
||||
b->plugin_instance = plugin_instance;
|
||||
b->plugin_instance_name = plugin_instance_name;
|
||||
|
||||
if (i==0 && !plugin_instance->config_attribs["face"].empty()) {
|
||||
return comperr(error, "Face attribute already exists in plugin", error_sz);
|
||||
}
|
||||
|
||||
b->plugin_instance->config_attribs["face"] = userface;
|
||||
|
||||
// update density
|
||||
if (dim == 2) {
|
||||
g->density *= volume[i] / (4./3. * mjPI * pow(g->size[0], 3));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (skin) {
|
||||
MakeSkin3(model);
|
||||
}
|
||||
@@ -2040,6 +2153,44 @@ void mjCComposite::MakeSkin3(mjCModel* model) {
|
||||
skin->inflate = skininflate;
|
||||
skin->group = skingroup;
|
||||
|
||||
// copy skin from existing mesh
|
||||
if (type==mjCOMPTYPE_PARTICLE && username.empty()) {
|
||||
std::vector<int> face;
|
||||
mjXUtil::String2Vector(userface, face);
|
||||
int nvert = uservert.size()/3;
|
||||
|
||||
for (int j=0; j<2; j++) {
|
||||
for (int i=0; i<nvert; i++) {
|
||||
skin->vert.push_back(0);
|
||||
skin->vert.push_back(0);
|
||||
skin->vert.push_back(0);
|
||||
|
||||
mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
|
||||
skin->bodyname.push_back(txt);
|
||||
skin->bindpos.push_back(0);
|
||||
skin->bindpos.push_back(0);
|
||||
skin->bindpos.push_back(0);
|
||||
skin->bindquat.push_back(1);
|
||||
skin->bindquat.push_back(0);
|
||||
skin->bindquat.push_back(0);
|
||||
skin->bindquat.push_back(0);
|
||||
|
||||
vector<int> vertid;
|
||||
vector<float> vertweight;
|
||||
vertid.push_back(j*nvert+i);
|
||||
vertweight.push_back(1);
|
||||
skin->vertid.push_back(vertid);
|
||||
skin->vertweight.push_back(vertweight);
|
||||
}
|
||||
|
||||
for (int i=0; i<face.size()/3; i++) {
|
||||
skin->face.push_back(j*nvert+face[3*i]-1);
|
||||
skin->face.push_back(j*nvert+face[3*i+(j==0 ? 1 : 2)]-1);
|
||||
skin->face.push_back(j*nvert+face[3*i+(j==0 ? 2 : 1)]-1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// box
|
||||
if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) {
|
||||
// z-faces
|
||||
|
||||
@@ -106,9 +106,13 @@ class mjCComposite {
|
||||
// currently used only for cable
|
||||
std::string initial; // root boundary type
|
||||
std::vector<float> uservert; // user-specified vertex positions
|
||||
std::string userface; // connectivity
|
||||
mjtNum size[3]; // rope size (meaning depends on the shape)
|
||||
mjtCompShape curve[3]; // geometric shape
|
||||
|
||||
// body names used in the skin
|
||||
std::vector<std::string> username;
|
||||
|
||||
// plugin support
|
||||
bool is_plugin;
|
||||
std::string plugin_name;
|
||||
|
||||
@@ -41,9 +41,6 @@ TEST_F(PluginTest, ElasticEnergy) {
|
||||
<composite type="particle" count="8 8 8" spacing="1">
|
||||
<geom size=".025" group="4"/>
|
||||
<plugin plugin="mujoco.elasticity.solid">
|
||||
<config key="nx" value="8"/>
|
||||
<config key="ny" value="8"/>
|
||||
<config key="nz" value="8"/>
|
||||
<config key="poisson" value="0"/>
|
||||
<config key="young" value="2"/>
|
||||
</plugin>
|
||||
|
||||
Reference in New Issue
Block a user