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

Add Flex component.

Browse Source 
PiperOrigin-RevId: 572830650
Change-Id: I6908228087b7b9683be3506c8d9cdc725ed5dcd5
This commit is contained in:
Alessio Quaglino
2023-10-12 10:15:46 +01:00
committed by Saran Tunyasuvunakool
parent 649a474788
commit 5a70ad08ab
82 changed files with 9658 additions and 1581 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/CMakeLists.txt
+2
View File
@@ -15,6 +15,8 @@
set(MUJOCO_USER_SRCS
user_composite.cc
user_composite.h
user_flexcomp.cc
user_flexcomp.h
user_mesh.cc
user_model.cc
user_model.h
src/user/user_flexcomp.cc
+1187
View File
File diff suppressed because it is too large Load Diff
src/user/user_flexcomp.h
+90
View File
@@ -0,0 +1,90 @@
// Copyright 2021 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_SRC_USER_USER_FLEXCOMP_H_
#define MUJOCO_SRC_USER_USER_FLEXCOMP_H_
#include <string>
#include <vector>
#include <mujoco/mujoco.h>
#include "user/user_model.h"
#include "user/user_objects.h"
typedef enum _mjtFcompType {
mjFCOMPTYPE_GRID = 0,
mjFCOMPTYPE_BOX,
mjFCOMPTYPE_CYLINDER,
mjFCOMPTYPE_ELLIPSOID,
mjFCOMPTYPE_MESH,
mjFCOMPTYPE_GMSH,
mjFCOMPTYPE_DIRECT,
mjNFCOMPTYPES
} mjtFcompType;
class mjCFlexcomp {
public:
mjCFlexcomp(void);
bool Make(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeGrid(char* error, int error_sz);
bool MakeBox(char* error, int error_sz);
bool MakeMesh(mjCModel* model, char* error, int error_sz);
bool MakeGMSH(mjCModel* model, char* error, int error_sz);
void LoadGMSH(mjCModel* model, mjResource* resource);
int GridID(int ix, int iy);
int GridID(int ix, int iy, int iz);
int BoxID(int ix, int iy, int iz);
void BoxProject(double* pos, int ix, int iy, int iz);
// common properties set by user
std::string name; // flex name
mjtFcompType type; // flexcomp type
int count[3]; // grid count in each dimension
double spacing[3]; // spacing between grid elements
double scale[3]; // scaling for mesh and direct
double mass; // total mass of auto-generated bodies
double inertiabox; // size of inertia box for each body
bool equality; // create edge equality constraint
std::string file; // mesh/gmsh file name
// pin specifications
std::vector<int> pinid; // ids of points to pin
std::vector<int> pinrange; // range of ids to pin
std::vector<int> pingrid; // grid coordinates to pin
std::vector<int> pingridrange; // range of grid coordinates to pin
// all other properties
mjCDef def; // local copy, parsed parameters stored here
// pose transform relative to parent body
double pos[3]; // position
double quat[4]; // orientation
mjCAlternative alt; // alternative orientation
// set by user or computed internally
bool rigid; // all vertices are in parent body (all pinned)
bool centered; // all vertex coordinates are (0,0,0) (nothing pinned)
std::vector<mjtNum> point; // flex bodies/vertices
std::vector<bool> pinned; // is point pinned (true: no new body)
std::vector<bool> used; // is point used by any element (false: skip)
std::vector<int> element; // flex elements
std::vector<float> texcoord; // vertex texture coordinates
};
#endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_
src/user/user_mesh.cc
+510 -22
View File
@@ -48,6 +48,7 @@
#include <mujoco/mjtnum.h>
#include <mujoco/mjplugin.h>
#include "engine/engine_crossplatform.h"
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_resource.h"
#include "engine/engine_util_blas.h"
@@ -139,8 +140,8 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
mjuu_setvec(boxsz_surface_, 0, 0, 0);
mjuu_setvec(boxsz_volume_, 0, 0, 0);
mjuu_setvec(aabb_, 1e10, 1e10, 1e10);
mjuu_setvec(aabb_+3, -1e10, -1e10, -1e10);
mjuu_setvec(aamm_, 1e10, 1e10, 1e10);
mjuu_setvec(aamm_+3, -1e10, -1e10, -1e10);
nvert_ = 0;
nnormal_ = 0;
ntexcoord_ = 0;
@@ -589,22 +590,22 @@ mjCBoundingVolume mjCMesh::GetBoundingVolume(int faceid) {
node.contype = 1;
node.pos = center_ + 3*faceid;
node.quat = NULL;
mjtNum AABB[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
mjtNum face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
for (int j=0; j<3; j++) {
int vertid = face_[3*faceid+j];
AABB[0] = mjMIN(AABB[0], vert_[3*vertid+0]);
AABB[1] = mjMIN(AABB[1], vert_[3*vertid+1]);
AABB[2] = mjMIN(AABB[2], vert_[3*vertid+2]);
AABB[3] = mjMAX(AABB[3], vert_[3*vertid+0]);
AABB[4] = mjMAX(AABB[4], vert_[3*vertid+1]);
AABB[5] = mjMAX(AABB[5], vert_[3*vertid+2]);
face_aamm[0] = mjMIN(face_aamm[0], vert_[3*vertid+0]);
face_aamm[1] = mjMIN(face_aamm[1], vert_[3*vertid+1]);
face_aamm[2] = mjMIN(face_aamm[2], vert_[3*vertid+2]);
face_aamm[3] = mjMAX(face_aamm[3], vert_[3*vertid+0]);
face_aamm[4] = mjMAX(face_aamm[4], vert_[3*vertid+1]);
face_aamm[5] = mjMAX(face_aamm[5], vert_[3*vertid+2]);
}
face_aabb_[6*faceid+0] = .5 * (AABB[0] + AABB[3]);
face_aabb_[6*faceid+1] = .5 * (AABB[1] + AABB[4]);
face_aabb_[6*faceid+2] = .5 * (AABB[2] + AABB[5]);
face_aabb_[6*faceid+3] = .5 * (AABB[3] - AABB[0]);
face_aabb_[6*faceid+4] = .5 * (AABB[4] - AABB[1]);
face_aabb_[6*faceid+5] = .5 * (AABB[5] - AABB[2]);
face_aabb_[6*faceid+0] = .5 * (face_aamm[0] + face_aamm[3]);
face_aabb_[6*faceid+1] = .5 * (face_aamm[1] + face_aamm[4]);
face_aabb_[6*faceid+2] = .5 * (face_aamm[2] + face_aamm[5]);
face_aabb_[6*faceid+3] = .5 * (face_aamm[3] - face_aamm[0]);
face_aabb_[6*faceid+4] = .5 * (face_aamm[4] - face_aamm[1]);
face_aabb_[6*faceid+5] = .5 * (face_aamm[5] - face_aamm[2]);
node.aabb = face_aabb_.data() + 6*faceid;
return node;
}
@@ -729,10 +730,10 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
}
}
// use aabb
// use aamm
else {
// find aabb box center
double cen[3] = {(aabb_[0]+aabb_[3])/2, (aabb_[1]+aabb_[4])/2, (aabb_[2]+aabb_[5])/2};
double cen[3] = {(aamm_[0]+aamm_[3])/2, (aamm_[1]+aamm_[4])/2, (aamm_[2]+aamm_[5])/2};
// add box center into meshpos
meshpos[0] += cen[0];
@@ -786,9 +787,9 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
case mjGEOM_ELLIPSOID:
case mjGEOM_BOX:
geom->size[0] = aabb_[3] - cen[0];
geom->size[1] = aabb_[4] - cen[1];
geom->size[2] = aabb_[5] - cen[2];
geom->size[0] = aamm_[3] - cen[0];
geom->size[1] = aamm_[4] - cen[1];
geom->size[2] = aamm_[5] - cen[2];
break;
default:
@@ -1399,8 +1400,8 @@ void mjCMesh::Process() {
vert_[3*i+j] = (float) res[j];
// axis-aligned bounding box
aabb_[j+0] = mju_min(aabb_[j+0], res[j]);
aabb_[j+3] = mju_max(aabb_[j+3], res[j]);
aamm_[j+0] = mju_min(aamm_[j+0], res[j]);
aamm_[j+3] = mju_max(aamm_[j+3], res[j]);
}
}
for (int i=0; i<nnormal_; i++) {
@@ -2151,3 +2152,490 @@ void mjCSkin::LoadSKN(mjResource* resource) {
throw mjCError(this, "unexpected buffer size in SKN file '%s'", resource->name);
}
}
//------------------ class mjCFlex implementation --------------------------------------------------
// constructor
mjCFlex::mjCFlex(mjCModel* _model) {
// set model
model = _model;
// set contact defaults
contype = 1;
conaffinity = 1;
condim = 3;
priority = 0;
mjuu_setvec(friction, 1, 0.005, 0.0001);
solmix = 1.0;
mj_defaultSolRefImp(solref, solimp);
margin = 0;
gap = 0;
// set other defaults
dim = 2;
radius = 0.005;
internal = true;
flatskin = false;
selfcollide = mjFLEXSELF_AUTO;
activelayers = 1;
group = 0;
edgestiffness = 0;
edgedamping = 0;
material.clear();
rgba[0] = rgba[1] = rgba[2] = 0.5f;
rgba[3] = 1.0f;
// clear internal variables
nvert = 0;
nedge = 0;
nelem = 0;
matid = -1;
rigid = false;
centered = false;
}
// compiler
void mjCFlex::Compile(const mjVFS* vfs) {
// set nelem; check sizes
if (dim<1 || dim>3) {
throw mjCError(this, "dim must be 1, 2 or 3");
}
if (elem.empty()) {
throw mjCError(this, "elem is empty");
}
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 (vert.size() % 3) {
throw mjCError(this, "vert size must be a multiple of 3");
}
nelem = (int)elem.size()/(dim+1);
// set nvert, rigid, centered; check size
if (vert.empty()) {
centered = true;
nvert = (int)vertbody.size();
}
else {
nvert = (int)vert.size()/3;
if (vertbody.size()==1) {
rigid = true;
}
}
if (nvert<dim+1) {
throw mjCError(this, "not enough vertices");
}
// check elem vertex ids
for (int i=0; i<(int)elem.size(); i++) {
if (elem[i]<0 || elem[i]>=nvert) {
throw mjCError(this, "elem vertex id out of range");
}
}
// check texcoord
if (!texcoord.empty() && texcoord.size()!=2*nvert) {
throw mjCError(this, "two texture coordinates per vertex expected");
}
// resolve material name
mjCBase* pmat = model->FindObject(mjOBJ_MATERIAL, material);
if (pmat) {
matid = pmat->id;
} else if (!material.empty()) {
throw mjCError(this, "unkown material '%s' in flex", material.c_str());
}
// resolve body ids
for (int i=0; i<(int)vertbody.size(); i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody[i]);
if (pbody) {
vertbodyid.push_back(pbody->id);
} else {
throw mjCError(this, "unkown body '%s' in flex", vertbody[i].c_str());
}
}
// process elements
for (int e=0; e<(int)elem.size()/(dim+1); e++) {
// make sorted copy of element
vector<int> el;
el.assign(elem.begin()+e*(dim+1), elem.begin()+(e+1)*(dim+1));
std::sort(el.begin(), el.end());
// check for repeated vertices
for (int k=0; k<dim; k++) {
if (el[k]==el[k+1]) {
throw mjCError(this, "repeated vertex in element");
}
}
// make edges from sorted element
switch (dim) {
case 1: // line
edge.push_back(std::make_pair(el[0], el[1]));
break;
case 2: // triangle
edge.push_back(std::make_pair(el[0], el[1]));
edge.push_back(std::make_pair(el[1], el[2]));
edge.push_back(std::make_pair(el[0], el[2]));
break;
case 3: // tetrahedron
edge.push_back(std::make_pair(el[0], el[1]));
edge.push_back(std::make_pair(el[1], el[2]));
edge.push_back(std::make_pair(el[2], el[3]));
edge.push_back(std::make_pair(el[0], el[2]));
edge.push_back(std::make_pair(el[0], el[3]));
edge.push_back(std::make_pair(el[1], el[3]));
break;
}
}
// sort edges
std::sort(edge.begin(), edge.end());
// remove repeated edges
std::vector<std::pair<int,int>> edge1;
edge1.push_back(edge[0]);
for (int i=1; i<(int)edge.size(); i++) {
if (edge1[edge1.size()-1]!=edge[i]) {
edge1.push_back(edge[i]);
}
}
edge = edge1;
// set size
nedge = (int)edge.size();
// determine rigid if not already set
if (!rigid) {
rigid = true;
for (int i=1; i<(int)vertbodyid.size(); i++) {
if (vertbodyid[i]!=vertbodyid[0]) {
rigid = false;
break;
}
}
}
// determine centered if not already set
if (!centered) {
centered = true;
for (int i=0; i<(int)vert.size(); i++) {
if (vert[i]!=0) {
centered = false;
break;
}
}
}
// compute global vertex positions
vertxpos = vector<mjtNum> (3*nvert);
for (int i=0; i<nvert; i++) {
// get body id, set vertxpos = body.xpos0
int b = rigid ? vertbodyid[0] : vertbodyid[i];
mju_copy3(vertxpos.data()+3*i, model->bodies[b]->xpos0);
// add vertex offset within body if not centered
if (!centered) {
mjtNum offset[3];
mju_rotVecQuat(offset, vert.data()+3*i, model->bodies[b]->xquat0);
mju_addTo3(vertxpos.data()+3*i, offset);
}
}
// reorder tetrahedra so right-handed face orientation is outside
// faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2)
if (dim==3) {
for (int e=0; e<nelem; e++) {
const int* edata = elem.data() + e*(dim+1);
mjtNum* v0 = vertxpos.data() + 3*edata[0];
mjtNum* v1 = vertxpos.data() + 3*edata[1];
mjtNum* v2 = vertxpos.data() + 3*edata[2];
mjtNum* v3 = vertxpos.data() + 3*edata[3];
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
mjtNum v03[3] = {v3[0]-v0[0], v3[1]-v0[1], v3[2]-v0[2]};
// detect wrong orientation
mjtNum nrm[3];
mju_cross(nrm, v01, v02);
if (mju_dot3(nrm, v03)>0) {
// flip orientation
int tmp = elem[e*(dim+1)+1];
elem[e*(dim+1)+1] = elem[e*(dim+1)+2];
elem[e*(dim+1)+2] = tmp;
}
}
}
// create shell fragments and element-vertex collision pairs
CreateShellPair();
// create bounding volume hierarchy
CreateBVH();
}
// create flex BVH
void mjCFlex::CreateBVH(void) {
// init bounding volume object
mjCBoundingVolume bv;
bv.contype = contype;
bv.conaffinity = conaffinity;
bv.quat = NULL;
// allocate element bounding boxes
vector<mjtNum> elemaabb(6*nelem);
// construct element bounding boxes, add to hierarchy
for (int e=0; e<nelem; e++) {
const int* edata = elem.data() + e*(dim+1);
// skip inactive in 3D
if (dim==3 && elemlayer[e]>=activelayers) {
continue;
}
// compute min and max along each global axis
mjtNum xmin[3], xmax[3];
mju_copy3(xmin, vertxpos.data() + 3*edata[0]);
mju_copy3(xmax, vertxpos.data() + 3*edata[0]);
for (int i=1; i<=dim; i++) {
for (int j=0; j<3; j++) {
xmin[j] = mjMIN(xmin[j], vertxpos[3*edata[i]+j]);
xmax[j] = mjMAX(xmax[j], vertxpos[3*edata[i]+j]);
}
}
// compute aabb (center, size)
elemaabb[6*e+0] = 0.5*(xmax[0]+xmin[0]);
elemaabb[6*e+1] = 0.5*(xmax[1]+xmin[1]);
elemaabb[6*e+2] = 0.5*(xmax[2]+xmin[2]);
elemaabb[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius;
elemaabb[6*e+4] = 0.5*(xmax[1]-xmin[1]) + radius;
elemaabb[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius;
// add bounding volume for this element
bv.id = e;
bv.aabb = elemaabb.data() + 6*e;
bv.pos = bv.aabb;
tree.AddBoundingVolume(bv);
}
// create hierarchy
tree.CreateBVH();
}
// create shells and element-vertex collision pairs
void mjCFlex::CreateShellPair(void) {
vector<vector<int>> fragspec(nelem*(dim+1)); // [sorted frag vertices, elem, original frag vertices]
vector<vector<int>> connectspec; // [elem1, elem2, common sorted frag vertices]
vector<bool> border(nelem, false); // is element on the border
vector<bool> borderfrag(nelem*(dim+1), false); // is fragment on the border
// make fragspec
for (int e=0; e<nelem; e++) {
int n = e*(dim+1);
// element vertices in original (unsorted) order
vector<int> el;
el.assign(elem.begin()+n, elem.begin()+n+dim+1);
// line: 2 vertex fragments
if (dim==1) {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
fragspec[n+1].push_back(el[1]);
fragspec[n+1].push_back(e);
fragspec[n+1].push_back(el[1]);
}
// triangle: 3 edge fragments
else if (dim==2) {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n+2].push_back(el[1]);
fragspec[n+2].push_back(el[2]);
fragspec[n+2].push_back(e);
fragspec[n+2].push_back(el[1]);
fragspec[n+2].push_back(el[2]);
fragspec[n+1].push_back(el[2]);
fragspec[n+1].push_back(el[0]);
fragspec[n+1].push_back(e);
fragspec[n+1].push_back(el[2]);
fragspec[n+1].push_back(el[0]);
}
// tetrahedron: 4 face fragments
else {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(el[2]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(el[2]);
fragspec[n+2].push_back(el[0]);
fragspec[n+2].push_back(el[2]);
fragspec[n+2].push_back(el[3]);
fragspec[n+2].push_back(e);
fragspec[n+2].push_back(el[0]);
fragspec[n+2].push_back(el[2]);
fragspec[n+2].push_back(el[3]);
fragspec[n+1].push_back(el[0]);
fragspec[n+1].push_back(el[3]);
fragspec[n+1].push_back(el[1]);
fragspec[n+1].push_back(e);
fragspec[n+1].push_back(el[0]);
fragspec[n+1].push_back(el[3]);
fragspec[n+1].push_back(el[1]);
fragspec[n+3].push_back(el[1]);
fragspec[n+3].push_back(el[3]);
fragspec[n+3].push_back(el[2]);
fragspec[n+3].push_back(e);
fragspec[n+3].push_back(el[1]);
fragspec[n+3].push_back(el[3]);
fragspec[n+3].push_back(el[2]);
}
}
// sort first segment of each fragspec
if (dim>1) {
for (int n=0; n<nelem*(dim+1); n++) {
std::sort(fragspec[n].begin(), fragspec[n].begin()+dim);
}
}
// sort fragspec
std::sort(fragspec.begin(), fragspec.end());
// make border and connectspec, record borderfrag
int cnt = 1;
for (int n=1; n<nelem*(dim+1); n++) {
// extract frag vertices, without elem
vector<int> previous = {fragspec[n-1].begin(), fragspec[n-1].begin()+dim};
vector<int> current = {fragspec[n].begin(), fragspec[n].begin()+dim};
// same sequential fragments
if (previous==current) {
// found pair of elements connected by common fragment
vector<int> connect;
connect.insert(connect.end(), fragspec[n-1][dim]);
connect.insert(connect.end(), fragspec[n][dim]);
connect.insert(connect.end(), fragspec[n].begin(), fragspec[n].begin()+dim);
connectspec.push_back(connect);
// count same sequential fragments
cnt++;
}
// different sequential fragments
else {
// found border fragment
if (cnt==1) {
border[fragspec[n-1][dim]] = true;
borderfrag[n-1] = true;
}
// reset count
cnt = 1;
}
}
// last fragment is border
if (cnt==1) {
int n = nelem*(dim+1);
border[fragspec[n-1][dim]] = true;
borderfrag[n-1] = true;
}
// create shell
for (int i=0; i<(int)borderfrag.size(); i++) {
if (borderfrag[i]) {
// add fragment vertices, in original order
shell.insert(shell.end(), fragspec[i].begin()+dim+1, fragspec[i].end());
}
}
// compute elemlayer (distance from border) via value iteration in 3D
if (dim<3) {
elemlayer = vector<int> (nelem, 0);
}
else {
elemlayer = vector<int> (nelem, nelem+1); // init with greater than max value
for (int e=0; e<nelem; e++) {
if (border[e]) {
elemlayer[e] = 0; // set border elements to 0
}
}
bool change = true;
while (change) { // repeat while changes are happening
change = false;
// process edges of element connectivity graph
for (int i=0; i<(int)connectspec.size(); i++) {
int e1 = connectspec[i][0]; // get element pair for this edge
int e2 = connectspec[i][1];
if (elemlayer[e1]>elemlayer[e2]+1) {
elemlayer[e1] = elemlayer[e2]+1; // better value found for e1: update
change = true;
} else if (elemlayer[e2]>elemlayer[e1]+1) {
elemlayer[e2] = elemlayer[e1]+1; // better value found for e2: update
change = true;
}
}
}
}
// create evpairs in 1D and 2D
if (dim<3) {
// process connected element pairs containing a border element
for (int n=0; n<(int)connectspec.size(); n++) {
if (border[connectspec[n][0]] || border[connectspec[n][1]]) {
// extract common fragment
vector<int> frag = {connectspec[n].begin()+2, connectspec[n].end()};
// process both elements
for (int ei=0; ei<2; ei++) {
const int* edata = elem.data() + connectspec[n][ei]*(dim+1);
// find element vertex that is not in the common fragment
for (int i=0; i<=dim; i++) {
if (frag.end() == std::find(frag.begin(), frag.end(), edata[i])) {
// add ev pair, involving the other element in connectspec
evpair.push_back(connectspec[n][1-ei]);
evpair.push_back(edata[i]);
// one such vertex exists
break;
}
}
}
}
}
}
}
src/user/user_model.cc
+225 -27
View File
@@ -139,6 +139,7 @@ mjCModel::mjCModel() {
//------------------------ private variables
cameras.clear();
lights.clear();
flexes.clear();
meshes.clear();
skins.clear();
hfields.clear();
@@ -185,6 +186,7 @@ mjCModel::~mjCModel() {
delete bodies[0];
// delete objects allocated in mjCModel
for (int i=0; i<flexes.size(); i++) delete flexes[i];
for (int i=0; i<meshes.size(); i++) delete meshes[i];
for (int i=0; i<skins.size(); i++) delete skins[i];
for (int i=0; i<hfields.size(); i++) delete hfields[i];
@@ -204,6 +206,7 @@ mjCModel::~mjCModel() {
for (int i=0; i<defaults.size(); i++) delete defaults[i];
// clear pointer lists created in model construction
flexes.clear();
meshes.clear();
skins.clear();
hfields.clear();
@@ -233,11 +236,14 @@ void mjCModel::Clear(void) {
// sizes set from list lengths
nbody = 0;
nbvh = 0;
nbvhstatic = 0;
nbvhdynamic = 0;
njnt = 0;
ngeom = 0;
nsite = 0;
ncam = 0;
nlight = 0;
nflex = 0;
nmesh = 0;
nskin = 0;
nhfield = 0;
@@ -256,6 +262,13 @@ void mjCModel::Clear(void) {
nv = 0;
nu = 0;
na = 0;
nflexvert = 0;
nflexedge = 0;
nflexelem = 0;
nflexelemdata = 0;
nflexshelldata = 0;
nflexevpair = 0;
nflextexcoord = 0;
nmeshvert = 0;
nmeshnormal = 0;
nmeshtexcoord = 0;
@@ -326,6 +339,12 @@ T* mjCModel::AddObjectDef(vector<T*>& list, string type, mjCDef* def) {
}
// add flex
mjCFlex* mjCModel::AddFlex(void) {
return AddObject(flexes, "flex");
}
// add mesh
mjCMesh* mjCModel::AddMesh(mjCDef* def) {
return AddObjectDef(meshes, "mesh", def);
@@ -443,6 +462,8 @@ int mjCModel::NumObjects(mjtObj type) {
return (int)cameras.size();
case mjOBJ_LIGHT:
return (int)lights.size();
case mjOBJ_FLEX:
return (int)flexes.size();
case mjOBJ_MESH:
return (int)meshes.size();
case mjOBJ_SKIN:
@@ -499,6 +520,8 @@ mjCBase* mjCModel::GetObject(mjtObj type, int id) {
return cameras[id];
case mjOBJ_LIGHT:
return lights[id];
case mjOBJ_FLEX:
return flexes[id];
case mjOBJ_MESH:
return meshes[id];
case mjOBJ_SKIN:
@@ -640,6 +663,8 @@ mjCBase* mjCModel::FindObject(mjtObj type, string name) {
return findobject(name, cameras);
case mjOBJ_LIGHT:
return findobject(name, lights);
case mjOBJ_FLEX:
return findobject(name, flexes);
case mjOBJ_MESH:
return findobject(name, meshes);
case mjOBJ_SKIN:
@@ -896,6 +921,7 @@ void mjCModel::SetSizes(void) {
nsite = (int)sites.size();
ncam = (int)cameras.size();
nlight = (int)lights.size();
nflex = (int)flexes.size();
nmesh = (int)meshes.size();
nskin = (int)skins.size();
nhfield = (int)hfields.size();
@@ -928,22 +954,38 @@ void mjCModel::SetSizes(void) {
}
}
// nbvh
// nbvh, nbvhstatic, nbvhdynamic
for (int i=0; i<nbody; i++) {
nbvh += bodies[i]->tree.nbvh;
nbvhstatic += bodies[i]->tree.nbvh;
}
for (int i=0; i<nmesh; i++) {
nbvhstatic += meshes[i]->tree().nbvh;
}
for (int i=0; i<nflex; i++) {
nbvhdynamic += flexes[i]->tree.nbvh;
}
nbvh = nbvhstatic + nbvhdynamic;
// flex counts
for (int i=0; i<nflex; i++) {
nflexvert += flexes[i]->nvert;
nflexedge += flexes[i]->nedge;
nflexelem += flexes[i]->nelem;
nflexelemdata += flexes[i]->nelem * (flexes[i]->dim + 1);
nflexshelldata += (int)flexes[i]->shell.size();
nflexevpair += (int)flexes[i]->evpair.size()/2;
}
// nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph
// mesh counts
for (int i=0; i<nmesh; i++) {
nmeshvert += meshes[i]->nvert();
nmeshnormal += meshes[i]->nnormal();
nmeshface += meshes[i]->nface();
nmeshtexcoord += (meshes[i]->HasTexcoord() ? meshes[i]->ntexcoord() : 0);
nmeshgraph += meshes[i]->szgraph();
nbvh += meshes[i]->tree().nbvh;
}
// nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert
// skin counts
for (int i=0; i<nskin; i++) {
nskinvert += skins[i]->vert.size()/3;
nskintexvert += skins[i]->texcoord.size()/2;
@@ -986,6 +1028,7 @@ void mjCModel::SetSizes(void) {
for (int i=0; i<nsite; i++) nnames += (int)sites[i]->name.length() + 1;
for (int i=0; i<ncam; i++) nnames += (int)cameras[i]->name.length() + 1;
for (int i=0; i<nlight; i++) nnames += (int)lights[i]->name.length() + 1;
for (int i=0; i<nflex; i++) nnames += (int)flexes[i]->name.length() + 1;
for (int i=0; i<nmesh; i++) nnames += (int)meshes[i]->name.length() + 1;
for (int i=0; i<nskin; i++) nnames += (int)skins[i]->name.length() + 1;
for (int i=0; i<nhfield; i++) nnames += (int)hfields[i]->name.length() + 1;
@@ -1271,6 +1314,9 @@ void mjCModel::CopyNames(mjModel* m) {
adr = namelist(lights, adr, m->name_lightadr, m->names, map_adr);
map_adr += mjLOAD_MULTIPLE*lights.size();
adr = namelist(flexes, adr, m->name_flexadr, m->names, map_adr);
map_adr += mjLOAD_MULTIPLE*flexes.size();
adr = namelist(meshes, adr, m->name_meshadr, m->names, map_adr);
map_adr += mjLOAD_MULTIPLE*meshes.size();
@@ -1382,13 +1428,17 @@ void mjCModel::CopyTree(mjModel* m) {
m->body_gravcomp[i] = pb->gravcomp;
copyvec(m->body_user+nuser_body*i, pb->userdata.data(), nuser_body);
m->body_contype[i] = pb->contype;
m->body_conaffinity[i] = pb->conaffinity;
m->body_margin[i] = (mjtNum)pb->margin;
// bounding volume hierarchy
m->body_bvhadr[i] = (!pb->geoms.empty() ? bvh_adr : -1);
m->body_bvhadr[i] = pb->tree.nbvh ? bvh_adr : -1;
m->body_bvhnum[i] = pb->tree.nbvh;
if (pb->tree.nbvh) {
memcpy(m->bvh_aabb + 6*bvh_adr, pb->tree.bvh.data(), 6*pb->tree.nbvh*sizeof(mjtNum));
memcpy(m->bvh_child + 2*bvh_adr, pb->tree.child.data(), 2*pb->tree.nbvh*sizeof(int));
memcpy(m->bvh_geomid + bvh_adr, pb->tree.nodeid.data(), pb->tree.nbvh*sizeof(int));
memcpy(m->bvh_nodeid + bvh_adr, pb->tree.nodeid.data(), pb->tree.nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pb->tree.level.data(), pb->tree.nbvh*sizeof(int));
}
bvh_adr += pb->tree.nbvh;
@@ -1753,7 +1803,8 @@ void mjCModel::CopyTree(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 bonevert_adr, graph_adr, data_adr, bvh_adr=0;
int edge_adr, elem_adr, elemdata_adr, shelldata_adr, evpair_adr;
int bonevert_adr, graph_adr, data_adr, bvh_adr;
// sizes outside call to mj_makeModel
m->nemax = nemax;
@@ -1762,15 +1813,18 @@ void mjCModel::CopyObjects(mjModel* m) {
m->nsensordata = nsensordata;
m->nuserdata = nuserdata;
// find bvh_adr after bodies
bvh_adr = 0;
for (int i=0; i<nbody; i++) {
bvh_adr = mjMAX(bvh_adr, m->body_bvhadr[i] + m->body_bvhnum[i]);
}
// meshes
vert_adr = 0;
normal_adr = 0;
texcoord_adr = 0;
face_adr = 0;
graph_adr = 0;
for (int i=0; i<nbody; i++) {
bvh_adr = mju_max(bvh_adr, m->body_bvhadr[i] + m->body_bvhnum[i]);
}
for (int i=0; i<nmesh; i++) {
// get pointer
mjCMesh* pme = meshes[i];
@@ -1785,8 +1839,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->mesh_faceadr[i] = face_adr;
m->mesh_facenum[i] = pme->nface();
m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1);
m->mesh_bvhadr[i] = bvh_adr;
m->mesh_bvhnum[i] = pme->tree().nbvh;
m->mesh_bvhadr[i] = pme->tree().nbvh ? bvh_adr : -1;
copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3);
copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4);
@@ -1804,10 +1858,14 @@ void mjCModel::CopyObjects(mjModel* m) {
if (pme->szgraph()) {
pme->CopyGraph(m->mesh_graph + graph_adr);
}
memcpy(m->bvh_aabb + 6*bvh_adr, pme->tree().bvh.data(), 6*pme->tree().nbvh*sizeof(mjtNum));
memcpy(m->bvh_child + 2*bvh_adr, pme->tree().child.data(), 2*pme->tree().nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pme->tree().level.data(), pme->tree().nbvh*sizeof(int));
memcpy(m->bvh_geomid + bvh_adr, pme->tree().nodeid.data(), pme->tree().nbvh*sizeof(int));
// copy bvh data
if (pme->tree().nbvh) {
memcpy(m->bvh_aabb + 6*bvh_adr, pme->tree().bvh.data(), 6*pme->tree().nbvh*sizeof(mjtNum));
memcpy(m->bvh_child + 2*bvh_adr, pme->tree().child.data(), 2*pme->tree().nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pme->tree().level.data(), pme->tree().nbvh*sizeof(int));
memcpy(m->bvh_nodeid + bvh_adr, pme->tree().nodeid.data(), pme->tree().nbvh*sizeof(int));
}
// advance counters
vert_adr += pme->nvert();
@@ -1818,6 +1876,127 @@ void mjCModel::CopyObjects(mjModel* m) {
bvh_adr += pme->tree().nbvh;
}
// flexes
vert_adr = 0;
edge_adr = 0;
elem_adr = 0;
elemdata_adr = 0;
shelldata_adr = 0;
evpair_adr = 0;
texcoord_adr = 0;
for (int i=0; i<nflex; i++) {
// get pointer
mjCFlex* pfl = flexes[i];
// set fields: geom-like
m->flex_contype[i] = pfl->contype;
m->flex_conaffinity[i] = pfl->conaffinity;
m->flex_condim[i] = pfl->condim;
m->flex_matid[i] = pfl->matid;
m->flex_group[i] = pfl->group;
m->flex_priority[i] = pfl->priority;
m->flex_solmix[i] = (mjtNum)pfl->solmix;
copyvec(m->flex_solref + mjNREF * i, pfl->solref, mjNREF);
copyvec(m->flex_solimp + mjNIMP * i, pfl->solimp, mjNIMP);
m->flex_radius[i] = (mjtNum)pfl->radius;
copyvec(m->flex_friction + 3 * i, pfl->friction, 3);
m->flex_margin[i] = (mjtNum)pfl->margin;
m->flex_gap[i] = (mjtNum)pfl->gap;
copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4);
// set fields: mesh-like
m->flex_dim[i] = pfl->dim;
m->flex_vertadr[i] = vert_adr;
m->flex_vertnum[i] = pfl->nvert;
m->flex_edgeadr[i] = edge_adr;
m->flex_edgenum[i] = pfl->nedge;
m->flex_elemadr[i] = elem_adr;
m->flex_elemdataadr[i] = elemdata_adr;
m->flex_shellnum[i] = (int)pfl->shell.size()/pfl->dim;
m->flex_shelldataadr[i] = m->flex_shellnum[i] ? shelldata_adr : -1;
if (pfl->evpair.empty()) {
m->flex_evpairadr[i] = -1;
m->flex_evpairnum[i] = 0;
} else {
m->flex_evpairadr[i] = evpair_adr;
m->flex_evpairnum[i] = (int)pfl->evpair.size()/2;
memcpy(m->flex_evpair + 2*evpair_adr, pfl->evpair.data(), pfl->evpair.size()*sizeof(int));
}
if (pfl->texcoord.empty()) {
m->flex_texcoordadr[i] = -1;
} else {
m->flex_texcoordadr[i] = texcoord_adr;
memcpy(m->flex_texcoord + 2*texcoord_adr,
pfl->texcoord.data(), pfl->texcoord.size()*sizeof(float));
}
m->flex_elemnum[i] = pfl->nelem;
memcpy(m->flex_elem + elemdata_adr, pfl->elem.data(), pfl->elem.size()*sizeof(int));
memcpy(m->flex_elemlayer + elem_adr, pfl->elemlayer.data(), pfl->nelem*sizeof(int));
if (m->flex_shellnum[i]) {
memcpy(m->flex_shell + shelldata_adr, pfl->shell.data(), pfl->shell.size()*sizeof(int));
}
m->flex_edgestiffness[i] = (mjtNum)pfl->edgestiffness;
m->flex_edgedamping[i] = (mjtNum)pfl->edgedamping;
m->flex_rigid[i] = pfl->rigid;
m->flex_centered[i] = pfl->centered;
m->flex_internal[i] = pfl->internal;
m->flex_flatskin[i] = pfl->flatskin;
m->flex_selfcollide[i] = pfl->selfcollide;
m->flex_activelayers[i] = pfl->activelayers;
m->flex_bvhnum[i] = pfl->tree.nbvh;
m->flex_bvhadr[i] = pfl->tree.nbvh ? bvh_adr : -1;
// find equality constraint referencing this flex
m->flex_edgeequality[i] = 0;
for (int k=0; k<(int)equalities.size(); k++) {
if (equalities[k]->type==mjEQ_FLEX && equalities[k]->name1==pfl->name) {
m->flex_edgeequality[i] = 1;
break;
}
}
// copy bvh data (flex aabb computed dynamically in mjData)
if (pfl->tree.nbvh) {
memcpy(m->bvh_child + 2*bvh_adr, pfl->tree.child.data(), 2*pfl->tree.nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pfl->tree.level.data(), pfl->tree.nbvh*sizeof(int));
memcpy(m->bvh_nodeid + bvh_adr, pfl->tree.nodeid.data(), pfl->tree.nbvh*sizeof(int));
}
// copy or set vert
if (pfl->centered) {
mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
}
else {
memcpy(m->flex_vert + 3*vert_adr, pfl->vert.data(), 3*pfl->nvert*sizeof(mjtNum));
}
// copy or set vertbodyid
if (pfl->rigid) {
for (int k=0; k<pfl->nvert; k++) {
m->flex_vertbodyid[vert_adr + k] = pfl->vertbodyid[0];
}
}
else {
memcpy(m->flex_vertbodyid + vert_adr, pfl->vertbodyid.data(), pfl->nvert*sizeof(int));
}
// convert edge pairs to int array
for (int i=0; i<pfl->nedge; i++) {
m->flex_edge[2*(edge_adr+i)] = pfl->edge[i].first;
m->flex_edge[2*(edge_adr+i)+1] = pfl->edge[i].second;
}
// advance counters
vert_adr += pfl->nvert;
edge_adr += pfl->nedge;
elem_adr += pfl->nelem;
elemdata_adr += (pfl->dim+1) * pfl->nelem;
shelldata_adr += (int)pfl->shell.size();
evpair_adr += (int)pfl->evpair.size()/2;
texcoord_adr += (int)pfl->texcoord.size()/2;
bvh_adr += pfl->tree.nbvh;
}
// skins
vert_adr = 0;
face_adr = 0;
@@ -1830,8 +2009,8 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields
m->skin_matid[i] = psk->matid;
copyvec(m->skin_rgba+4*i, psk->rgba, 4);
m->skin_group[i] = psk->group;
copyvec(m->skin_rgba+4*i, psk->rgba, 4);
m->skin_inflate[i] = psk->inflate;
m->skin_vertadr[i] = vert_adr;
m->skin_vertnum[i] = psk->vert.size()/3;
@@ -2412,7 +2591,6 @@ void mjCModel::FuseStatic(void) {
//------------------------------- COMPILER ---------------------------------------------------------
// signature comparisons
@@ -2445,13 +2623,25 @@ static void processlist(vector<T*>& list, string defname, bool checkrepeat=true)
// id equals position in array
list[i]->id = i;
}
// compare to all previous names
if (checkrepeat) {
for (int j=0; j<i; j++) {
if (list[i]->name == list[j]->name && list[j]->name != "") {
throw mjCError(list[i], "repeated name in %s array, position %d", defname.c_str(), i);
}
// check for repeated names
if (checkrepeat) {
// created vectors with all names
vector<string> allnames;
for (int i=0; i<(int)list.size(); i++) {
if (!list[i]->name.empty()) {
allnames.push_back(list[i]->name);
}
}
// sort and check for duplicates
if (allnames.size()>1) {
std::sort(allnames.begin(), allnames.end());
auto adjacent = std::adjacent_find(allnames.begin(), allnames.end());
if (adjacent!=allnames.end()) {
string msg = "repeated name '" + *adjacent + "' in " + defname;
throw mjCError(NULL, msg.c_str());
}
}
}
@@ -2549,6 +2739,11 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
throw mjCError(0, "joint found in world body");
}
// check for too many body+flex
if (bodies.size()+flexes.size()>=65534) {
throw mjCError(0, "number of bodies plus flexes must be less than 65534");
}
// append directory separator
if (!meshdir.empty()) {
int n = meshdir.length();
@@ -2578,6 +2773,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
processlist(sites, "site");
processlist(cameras, "camera");
processlist(lights, "light");
processlist(flexes, "flex");
processlist(meshes, "mesh");
processlist(skins, "skin");
processlist(hfields, "hfield");
@@ -2668,6 +2864,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
// compile all other objects except for keyframes
for (int i=0; i<flexes.size(); i++) flexes[i]->Compile(vfs);
for (int i=0; i<skins.size(); i++) skins[i]->Compile(vfs);
for (int i=0; i<hfields.size(); i++) hfields[i]->Compile(vfs);
for (int i=0; i<textures.size(); i++) textures[i]->Compile(vfs);
@@ -2744,12 +2941,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
// create low-level model
m = mj_makeModel(nq, nv, nu, na, nbody, nbvh, njnt, ngeom, nsite, ncam, nlight,
m = mj_makeModel(nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite,
ncam, nlight, nflex, nflexvert, nflexedge, nflexelem,
nflexelemdata, nflexshelldata, nflexevpair, nflextexcoord,
nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph,
nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert,
nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude,
neq, ntendon, nwrap, nsensor,
nnumeric, nnumericdata, ntext, ntextdata,
neq, ntendon, nwrap, nsensor, nnumeric, nnumericdata, ntext, ntextdata,
ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr,
nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam,
nuser_tendon, nuser_actuator, nuser_sensor, nnames, npaths);
src/user/user_model.h
+14 -1
View File
@@ -44,6 +44,7 @@ class mjCModel {
friend class mjCBody;
friend class mjCJoint;
friend class mjCGeom;
friend class mjCFlex;
friend class mjCMesh;
friend class mjCSkin;
friend class mjCHField;
@@ -72,6 +73,7 @@ class mjCModel {
//------------------------ API for adding model elements
mjCFlex* AddFlex(void); // flex
mjCMesh* AddMesh(mjCDef* def = 0); // mesh
mjCSkin* AddSkin(void); // skin
mjCHField* AddHField(void); // heightfield
@@ -194,6 +196,7 @@ class mjCModel {
int nsite; // number of sites
int ncam; // number of cameras
int nlight; // number of lights
int nflex; // number of flexes
int nmesh; // number of meshes
int nskin; // number of skins
int nhfield; // number of height fields
@@ -216,7 +219,16 @@ class mjCModel {
int nv; // number of degrees of freedom = dim(qvel)
int nu; // number of actuators/controls
int na; // number of activation variables
int nbvh; // number of boundary volume hierarchies
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 nflexvert; // number of vertices in all flexes
int nflexedge; // number of edges in all flexes
int nflexelem; // number of elements in all flexes
int nflexelemdata; // number of element vertex ids in all flexes
int nflexshelldata; // number of shell fragment vertex ids in all flexes
int nflexevpair; // number of element-vertex pairs in all flexes
int nflextexcoord; // number of vertex texture coordinates in all flexes
int nmeshvert; // number of vertices in all meshes
int nmeshnormal; // number of normals in all meshes
int nmeshtexcoord; // number of texture coordinates in all meshes
@@ -243,6 +255,7 @@ class mjCModel {
//------------------------ object lists
// objects created here
std::vector<mjCFlex*> flexes; // list of flexes
std::vector<mjCMesh*> meshes; // list of meshes
std::vector<mjCSkin*> skins; // list of skins
std::vector<mjCHField*> hfields; // list of height fields
src/user/user_objects.cc
+111 -73
View File
@@ -296,34 +296,39 @@ void mjCBoundingVolumeHierarchy::CreateBVH() {
// compute bounding volume hierarchy
int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev) {
if (elements.empty()) {
return -1;
}
int nelements = elements.size();
mjtNum AABB[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
mjtNum AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
// inverse transformation
mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
// accumulate AAMM over elements
for (int i=0; i<nelements; i++) {
// skip visual objects
if (elements[i].conaffinity==0 && elements[i].contype==0) {
continue;
}
// transform aabb representation
mjtNum aabb[6] = {elements[i].aabb[0] - elements[i].aabb[3],
// transform element aabb to aamm format
mjtNum aamm[6] = {elements[i].aabb[0] - elements[i].aabb[3],
elements[i].aabb[1] - elements[i].aabb[4],
elements[i].aabb[2] - elements[i].aabb[5],
elements[i].aabb[0] + elements[i].aabb[3],
elements[i].aabb[1] + elements[i].aabb[4],
elements[i].aabb[2] + elements[i].aabb[5]};
// update node AABB
// update node AAMM
for (int v=0; v<8; v++) {
mjtNum vert[3], box[3];
vert[0] = (v&1 ? aabb[3] : aabb[0]);
vert[1] = (v&2 ? aabb[4] : aabb[1]);
vert[2] = (v&4 ? aabb[5] : aabb[2]);
vert[0] = (v&1 ? aamm[3] : aamm[0]);
vert[1] = (v&2 ? aamm[4] : aamm[1]);
vert[2] = (v&4 ? aamm[5] : aamm[2]);
// rotate to the body inertial frame
// rotate to the body inertial frame if specified
if (elements[i].quat) {
mju_rotVecQuat(box, vert, elements[i].quat);
box[0] += elements[i].pos[0] - ipos_[0];
@@ -332,20 +337,20 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
mju_rotVecQuat(vert, box, qinv);
}
AABB[0] = mjMIN(AABB[0], vert[0]);
AABB[1] = mjMIN(AABB[1], vert[1]);
AABB[2] = mjMIN(AABB[2], vert[2]);
AABB[3] = mjMAX(AABB[3], vert[0]);
AABB[4] = mjMAX(AABB[4], vert[1]);
AABB[5] = mjMAX(AABB[5], vert[2]);
AAMM[0] = mjMIN(AAMM[0], vert[0]);
AAMM[1] = mjMIN(AAMM[1], vert[1]);
AAMM[2] = mjMIN(AAMM[2], vert[2]);
AAMM[3] = mjMAX(AAMM[3], vert[0]);
AAMM[4] = mjMAX(AAMM[4], vert[1]);
AAMM[5] = mjMAX(AAMM[5], vert[2]);
}
}
// inflate flat AABBs
for (int i=0; i<3; i++) {
if (mju_abs(AABB[i]-AABB[i+3])<mjEPS) {
AABB[i+0] -= mjEPS;
AABB[i+3] += mjEPS;
if (mju_abs(AAMM[i]-AAMM[i+3])<mjEPS) {
AAMM[i+0] -= mjEPS;
AAMM[i+3] += mjEPS;
}
}
@@ -358,10 +363,10 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
// store bounding box of the current node
for (int i=0; i<3; i++) {
bvh.push_back((AABB[3+i] + AABB[i]) / 2);
bvh.push_back((AAMM[3+i] + AAMM[i]) / 2);
}
for (int i=0; i<3; i++) {
bvh.push_back((AABB[3+i] - AABB[i]) / 2);
bvh.push_back((AAMM[3+i] - AAMM[i]) / 2);
}
// leaf node, return
@@ -374,7 +379,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
}
// find longest axis for splitting the bounding box
mjtNum edges[3] = { AABB[3]-AABB[0], AABB[4]-AABB[1], AABB[5]-AABB[2] };
mjtNum edges[3] = { AAMM[3]-AAMM[0], AAMM[4]-AAMM[1], AAMM[5]-AAMM[2] };
int axis = edges[0] > edges[1] ? 0 : 1;
axis = edges[axis] > edges[2] ? axis : 2;
@@ -505,7 +510,7 @@ mjResource* mjCBase::LoadResource(string filename, const mjVFS* vfs) {
if ((r = mju_openVfsResource(cname, vfs)) == nullptr) {
// not in vfs try a provider or fallback to OS filesystem
if ((r = mju_openResource(filename.c_str())) == nullptr) {
throw mjCError(this, "resource not found via provider or OS filesystem: '%s'", cname);
throw mjCError(nullptr, "resource not found via provider or OS filesystem: '%s'", cname);
}
}
return r;
@@ -560,6 +565,12 @@ mjCBody::mjCBody(mjCModel* _model) {
gravcomp = 0;
userdata.clear();
contype = 0;
conaffinity = 0;
margin = 0;
mjuu_zerovec(xpos0, 3);
mjuu_setvec(xquat0, 1, 0, 0, 0);
// clear object lists
bodies.clear();
geoms.clear();
@@ -984,6 +995,15 @@ void mjCBody::Compile(void) {
MakeLocal(geoms[i]->locpos, geoms[i]->locquat, geoms[i]->pos, geoms[i]->quat);
}
// accumulate rbound, contype, conaffinity over geoms
contype = conaffinity = 0;
margin = 0;
for (int i=0; i<geoms.size(); i++) {
contype |= geoms[i]->contype;
conaffinity |= geoms[i]->conaffinity;
margin = mju_max(margin, geoms[i]->margin);
}
// compute bounding volume hierarchy
if (!geoms.empty()) {
tree.Set(ipos, iquat);
@@ -1021,6 +1041,14 @@ void mjCBody::Compile(void) {
throw mjCError(this, "mocap body '%s' is not a fixed child of world", name.c_str());
}
// compute body global pose (no joint transformations in qpos0)
if (id>0) {
mjCBody* par = model->bodies[parentid];
mju_rotVecQuat(xpos0, locpos, par->xquat0);
mju_addTo3(xpos0, par->xpos0);
mju_mulQuat(xquat0, par->xquat0, locquat);
}
// compile all sites
for (int i=0; i<sites.size(); i++) sites[i]->Compile();
@@ -1329,6 +1357,7 @@ double mjCGeom::GetVolume(void) {
case mjGEOM_ELLIPSOID:
return 4*mjPI*size[0]*size[1]*size[2]/3;
case mjGEOM_HFIELD:
case mjGEOM_BOX:
return size[0]*size[1]*size[2]*8;
@@ -1408,6 +1437,7 @@ void mjCGeom::SetInertia(void) {
inertia[2] = mass*(size[0]*size[0]+size[1]*size[1])/5;
return;
case mjGEOM_HFIELD:
case mjGEOM_BOX:
inertia[0] = mass*(size[1]*size[1]+size[2]*size[2])/3;
inertia[1] = mass*(size[0]*size[0]+size[2]*size[2])/3;
@@ -1425,10 +1455,15 @@ void mjCGeom::SetInertia(void) {
// compute radius of bounding sphere
double mjCGeom::GetRBound(void) {
const double* aabb;
const double *aamm, *hsize;
double haabb[3] = {0};
switch (type) {
case mjGEOM_HFIELD:
hsize = model->hfields[hfieldid]->size;
return sqrt(hsize[0]*hsize[0] + hsize[1]*hsize[1] +
mjMAX(hsize[2]*hsize[2], hsize[3]*hsize[3]));
case mjGEOM_SPHERE:
return size[0];
@@ -1446,10 +1481,10 @@ double mjCGeom::GetRBound(void) {
case mjGEOM_MESH:
case mjGEOM_SDF:
aabb = model->meshes[meshid]->aabb();
haabb[0] = mju_max(fabs(aabb[0]), fabs(aabb[3]));
haabb[1] = mju_max(fabs(aabb[1]), fabs(aabb[4]));
haabb[2] = mju_max(fabs(aabb[2]), fabs(aabb[5]));
aamm = model->meshes[meshid]->aamm();
haabb[0] = mju_max(fabs(aamm[0]), fabs(aamm[3]));
haabb[1] = mju_max(fabs(aamm[1]), fabs(aamm[4]));
haabb[2] = mju_max(fabs(aamm[2]), fabs(aamm[5]));
return sqrt(haabb[0]*haabb[0] + haabb[1]*haabb[1] + haabb[2]*haabb[2]);
default:
@@ -1571,68 +1606,63 @@ void mjCGeom::SetFluidCoefs(void) {
// compute bounding box
void mjCGeom::ComputeAABB() {
void mjCGeom::ComputeAABB(void) {
double aamm[6]; // axis-aligned bounding box in (min, max) format
switch (type) {
case mjGEOM_HFIELD:
aamm[0] = -model->hfields[hfieldid]->size[0];
aamm[1] = -model->hfields[hfieldid]->size[1];
aamm[2] = -model->hfields[hfieldid]->size[3];
aamm[3] = model->hfields[hfieldid]->size[0];
aamm[4] = model->hfields[hfieldid]->size[1];
aamm[5] = model->hfields[hfieldid]->size[2];
break;
case mjGEOM_SPHERE:
aabb[3] = aabb[4] = aabb[5] = size[0];
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
aamm[3] = aamm[4] = aamm[5] = size[0];
mjuu_setvec(aamm, -aamm[3], -aamm[4], -aamm[5]);
break;
case mjGEOM_CAPSULE:
aabb[3] = aabb[4] = size[0];
aabb[5] = size[0] + size[1];
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
aamm[3] = aamm[4] = size[0];
aamm[5] = size[0] + size[1];
mjuu_setvec(aamm, -aamm[3], -aamm[4], -aamm[5]);
break;
case mjGEOM_CYLINDER:
aabb[3] = aabb[4] = size[0];
aabb[5] = size[1];
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
aamm[3] = aamm[4] = size[0];
aamm[5] = size[1];
mjuu_setvec(aamm, -aamm[3], -aamm[4], -aamm[5]);
break;
case mjGEOM_MESH:
case mjGEOM_SDF:
mjuu_copyvec(aabb, model->meshes[meshid]->aabb(), 6);
mjuu_copyvec(aamm, model->meshes[meshid]->aamm(), 6);
break;
case mjGEOM_PLANE:
aabb[0] = aabb[1] = aabb[2] = -mjMAXVAL;
aabb[3] = aabb[4] = mjMAXVAL;
aabb[5] = 0;
break;
case mjGEOM_HFIELD:
aabb[0] = -size[0];
aabb[1] = -size[1];
aabb[2] = -model->hfields[hfieldid]->size[3];
aabb[3] = size[0];
aabb[4] = size[1];
aabb[5] = model->hfields[hfieldid]->size[2];
aamm[0] = aamm[1] = aamm[2] = -mjMAXVAL;
aamm[3] = aamm[4] = mjMAXVAL;
aamm[5] = 0;
break;
default:
mjuu_copyvec(aabb+3, size, 3);
mjuu_setvec(aabb, -size[0], -size[1], -size[2]);
mjuu_copyvec(aamm+3, size, 3);
mjuu_setvec(aamm, -size[0], -size[1], -size[2]);
break;
}
aabb[0] -= margin;
aabb[1] -= margin;
aabb[2] -= margin;
aabb[3] += margin;
aabb[4] += margin;
aabb[5] += margin;
mjtNum pos[] = {(aabb[3] + aabb[0]) / 2, (aabb[4] + aabb[1]) / 2,
(aabb[5] + aabb[2]) / 2};
mjtNum size[] = {(aabb[3] - aabb[0]) / 2, (aabb[4] - aabb[1]) / 2,
(aabb[5] - aabb[2]) / 2};
// convert aamm to aabb (center, size) format
double pos[] = {(aamm[3] + aamm[0]) / 2, (aamm[4] + aamm[1]) / 2,
(aamm[5] + aamm[2]) / 2};
double size[] = {(aamm[3] - aamm[0]) / 2, (aamm[4] - aamm[1]) / 2,
(aamm[5] - aamm[2]) / 2};
mjuu_copyvec(aabb, pos, 3);
mjuu_copyvec(aabb+3, size, 3);
}
// compiler
void mjCGeom::Compile(void) {
// resize userdata
@@ -1662,9 +1692,9 @@ void mjCGeom::Compile(void) {
throw mjCError(this, "hfield geom '%s' (id = %d) must have valid hfieldid", name.c_str(), id);
}
// plane and hfield only allowed in static bodies
if ((type==mjGEOM_PLANE || type==mjGEOM_HFIELD) && body->weldid!=0) {
throw mjCError(this, "plane and hfield only allowed in static bodies: geom '%s' (id = %d)",
// plane only allowed in static bodies
if (type==mjGEOM_PLANE && body->weldid!=0) {
throw mjCError(this, "plane only allowed in static bodies: geom '%s' (id = %d)",
name.c_str(), id);
}
@@ -1759,12 +1789,13 @@ void mjCGeom::Compile(void) {
if (type==mjGEOM_HFIELD) {
size[0] = model->hfields[hfieldid]->size[0];
size[1] = model->hfields[hfieldid]->size[1];
size[2] = 0.5*(model->hfields[hfieldid]->size[2]+model->hfields[hfieldid]->size[3]);
size[2] = 0.5*(0.5*model->hfields[hfieldid]->size[2] +
model->hfields[hfieldid]->size[3]);
} else if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
const double* aabb = model->meshes[meshid]->aabb();
size[0] = mju_max(fabs(aabb[0]), fabs(aabb[3]));
size[1] = mju_max(fabs(aabb[1]), fabs(aabb[4]));
size[2] = mju_max(fabs(aabb[2]), fabs(aabb[5]));
const double* aamm = model->meshes[meshid]->aamm();
size[0] = mju_max(fabs(aamm[0]), fabs(aamm[3]));
size[1] = mju_max(fabs(aamm[1]), fabs(aamm[4]));
size[2] = mju_max(fabs(aamm[2]), fabs(aamm[5]));
}
for (double s : size) {
@@ -3120,7 +3151,7 @@ void mjCPair::Compile(void) {
// get geom ids and body signature
geom1 = pg1->id;
geom2 = pg2->id;
signature = ((pg1->body->id+1)<<16) + pg2->body->id+1;
signature = ((pg1->body->id)<<16) + pg2->body->id;
// set undefined margin: max
if (!mjuu_defined(margin)) {
@@ -3262,7 +3293,7 @@ void mjCBodyPair::Compile(void) {
// get body ids and body signature
body1 = pb1->id;
body2 = pb2->id;
signature = ((body1+1)<<16) + body2+1;
signature = (body1<<16) + body2;
}
@@ -3310,6 +3341,8 @@ void mjCEquality::Compile(void) {
objtype = mjOBJ_JOINT;
} else if (type==mjEQ_TENDON) {
objtype = mjOBJ_TENDON;
} else if (type==mjEQ_FLEX) {
objtype = mjOBJ_FLEX;
} else {
throw mjCError(this, "invalid type in equality constraint '%s' (id = %d)'", name.c_str(), id);
}
@@ -3330,7 +3363,7 @@ void mjCEquality::Compile(void) {
obj2id = px2->id;
}
// object 2 unspecified: set to -1, except for distance
// object 2 unspecified: set to -1
else {
if (objtype==mjOBJ_GEOM) {
throw mjCError(this, "both geom are required in equality constraint '%s' (id = %d)",
@@ -3346,6 +3379,11 @@ void mjCEquality::Compile(void) {
obj2id = 0;
}
// make sure flex is not rigid
if (type==mjEQ_FLEX && model->flexes[obj1id]->rigid) {
throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1.c_str(), id);
}
// make sure the two objects are different
if (obj1id==obj2id) {
throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1.c_str(), id);
src/user/user_objects.h
+86 -17
View File
@@ -35,8 +35,9 @@ class mjCGeom;
class mjCSite;
class mjCCamera;
class mjCLight;
class mjCMesh;
class mjCSkin;
class mjCFlex; // defined in user_mesh
class mjCMesh; // defined in user_mesh
class mjCSkin; // defined in user_mesh
class mjCTexture;
class mjCMaterial;
class mjCPair;
@@ -50,7 +51,6 @@ class mjCNumeric;
class mjCText;
class mjCTuple;
class mjCDef;
class mjCMesh; // defined in user_mesh
class mjCModel; // defined in user_model
class mjXWriter; // defined in xml_native
class mjXURDF; // defined in xml_urdf
@@ -118,10 +118,6 @@ class mjCAlternative {
//------------------------- class mjCBoundingVolumeHierarchy ---------------------------------------
// bounding volume
@@ -132,7 +128,7 @@ class mjCBoundingVolume {
int id; // object id
int contype; // contact type
int conaffinity; // contact affinity
const mjtNum* aabb; // half-sizes of axis-aligned bounding box
const mjtNum* aabb; // axis-aligned bounding box (center, size)
const mjtNum* pos; // position (set by user or Compile1)
const mjtNum* quat; // orientation (set by user or Compile1)
};
@@ -146,11 +142,11 @@ class mjCBoundingVolumeHierarchy {
int nbvh;
std::vector<mjtNum> bvh; // bounding boxes (nbvh x 6)
std::vector<int> child; // children of each node (nbvh x 2)
std::vector<int> nodeid; // id of the geom contained by the node (nbvh x 1)
std::vector<int> nodeid; // geom of elem id contained by the node (nbvh x 1)
std::vector<int> level; // levels of each node (nbvh x 1)
// make bounding volume hierarchy
void CreateBVH();
void CreateBVH(void);
void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]);
void AddBoundingVolume(const mjCBoundingVolume& bv);
@@ -174,7 +170,7 @@ class mjCBase {
public:
// load resource if found (fallback to OS filesystem)
mjResource* LoadResource(std::string filename, const mjVFS* vfs);
static mjResource* LoadResource(std::string filename, const mjVFS* vfs);
// Get and sanitize content type from raw_text if not empty, otherwise parse
// content type from resource_name; throw on failure
@@ -208,6 +204,7 @@ class mjCBody : public mjCBase {
friend class mjCSite;
friend class mjCCamera;
friend class mjCLight;
friend class mjCFlex;
friend class mjCEquality;
friend class mjCPair;
friend class mjCModel;
@@ -266,6 +263,12 @@ class mjCBody : public mjCBase {
int mocapid; // mocap id, -1: not mocap
bool explicitinertial; // whether to save the body with an explicit inertial clause
int contype; // OR over geom contypes
int conaffinity; // OR over geom conaffinities
double margin; // MAX over geom margins
mjtNum xpos0[3]; // global position in qpos0
mjtNum xquat0[4]; // global orientation in qpos0
// used internally by compiler
int lastdof; // id of last dof
int subtreedofs; // number of dofs in subtree, including self
@@ -389,7 +392,7 @@ class mjCGeom : public mjCBase {
double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid
mjCAlternative alt; // alternative orientation specifications
// variables set by user or 'Compile1'
// variables set by user or 'Compile'
double pos[3]; // position
double quat[4]; // orientation
@@ -406,7 +409,7 @@ class mjCGeom : public mjCBase {
double inertia[3]; // local diagonal inertia
double locpos[3]; // local position
double locquat[4]; // local orientation
double aabb[6]; // half-sizes of axis-aligned bounding box
double aabb[6]; // axis-aligned bounding box (center, size)
mjCBody* body; // geom's body
};
@@ -524,10 +527,75 @@ class mjCLight : public mjCBase {
//------------------------- class mjCFlex ----------------------------------------------------------
// Describes a flex
class mjCFlex: public mjCBase {
friend class mjCDef;
friend class mjCModel;
friend class mjCFlexcomp;
friend class mjCEquality;
friend class mjXWriter;
public:
// contact properties
int contype; // contact type
int conaffinity; // contact affinity
int condim; // contact dimensionality
int priority; // contact priority
double friction[3]; // one-sided friction coefficients: slide, roll, spin
double solmix; // solver mixing for contact pairs
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
// other properties
int dim; // element dimensionality
double radius; // radius around primitive element
bool internal; // enable internal collisions
bool flatskin; // render flex skin with flat shading
int selfcollide; // mode for flex self colllision
int activelayers; // number of active element layers in 3D
int group; // group for visualizatioh
double edgestiffness; // edge stiffness
double edgedamping; // edge damping
std::string material; // name of material used for rendering
float rgba[4]; // rgba when material is omitted
std::vector<std::string> vertbody; // vertex body names
std::vector<mjtNum> vert; // vertex positions
std::vector<int> elem; // element vertex ids
std::vector<float> texcoord; // vertex texture coordinates
private:
mjCFlex(mjCModel* = 0); // constructor
void Compile(const mjVFS* vfs); // compiler
void CreateBVH(void); // create flex BVH
void CreateShellPair(void); // create shells and evpairs
int nvert; // number of verices
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)
std::vector<int> vertbodyid; // vertex 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)
std::vector<int> evpair; // element-vertex pairs
std::vector<mjtNum> vertxpos; // global vertex positions
mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy
};
//------------------------- class mjCMesh ----------------------------------------------------------
// Describes a mesh
class mjCMesh: public mjCBase {
friend class mjCFlexcomp;
public:
mjCMesh(mjCModel* = 0, mjCDef* = 0);
~mjCMesh();
@@ -548,7 +616,7 @@ class mjCMesh: public mjCBase {
// mesh properites computed by Compile
const double* boxsz_volume() const { return boxsz_volume_; }
const double* aabb() const { return aabb_; }
const double* aamm() const { return aamm_; }
// number of vertices, normals, texture coordinates, and faces
int nvert() const { return nvert_; }
@@ -652,7 +720,7 @@ class mjCMesh: public mjCBase {
double quat_[4]; // rotation applied to asset vertices
double boxsz_volume_[3]; // half-sizes of equivalent inertia box (volume)
double boxsz_surface_[3]; // half-sizes of equivalent inertia box (surface)
double aabb_[6]; // axis-aligned bounding box
double aamm_[6]; // axis-aligned bounding box in (min, max) format
double volume_; // volume of the mesh
double surface_; // surface of the mesh
@@ -864,7 +932,7 @@ class mjCPair : public mjCBase {
int geom1; // id of geom1
int geom2; // id of geom2
int signature; // (body1+1)<<16 + body2+1
int signature; // body1<<16 + body2
};
@@ -891,7 +959,7 @@ class mjCBodyPair : public mjCBase {
int body1; // id of body1
int body2; // id of body2
int signature; // (body1+1)<<16 + body2+1
int signature; // body1<<16 + body2
};
@@ -1199,6 +1267,7 @@ class mjCDef {
mjCSite site;
mjCCamera camera;
mjCLight light;
mjCFlex flex;
mjCMesh mesh;
mjCMaterial material;
mjCPair pair;