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/engine/engine_collision_box.c
+35 -22
View File
@@ -16,6 +16,7 @@
#include <string.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mujoco.h>
#include "engine/engine_collision_primitive.h"
#include "engine/engine_util_blas.h"
@@ -36,16 +37,16 @@ static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n)
}
static int _SphereBox(mjContact* con, mjtNum mindist,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2)
// raw sphere : box
int mjraw_SphereBox(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2)
{
int i, k;
mjtNum tmp[3], center[3], clamped[3], deepest[3], nearest[3];
mjtNum pos[3];
mjtNum dist, closest;
mju_sub3(tmp, pos1, pos2);
mju_rotVecMatT(center, tmp, mat2);
@@ -56,10 +57,9 @@ static int _SphereBox(mjContact* con, mjtNum mindist,
mju_sub3(tmp, clamped, center);
dist = mju_normalize3(tmp);
if (dist - size1[0] > mindist)
if (dist - size1[0] > margin)
return 0;
// sphere center inside box
if (dist <= mjMINVAL) {
closest = (size2[0] + size2[1] + size2[2]) * 2;
@@ -93,32 +93,36 @@ static int _SphereBox(mjContact* con, mjtNum mindist,
return 1;
}
// sphere : box
int mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum margin)
{
mjGETINFO;
return _SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
return mjraw_SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
// GENERAL THEORY OF OPERATION
// the following code is mostly for finding (line segment)/(box) collision
// after which box-sphere is called
// First the closest point to the box is found.
// Then a "sensible" second point is found if the angle
// between the segment and the box is low enough < 45
/* GENERAL THEORY OF OPERATION
the following code is mostly for finding (line segment)/(box) collision
after which box-sphere is called
// In the comments that follow, capsule just means the capsule's line segment
// It might be hard to understand all comments but you would need
// a picture to see what is happening at each line of the code
First the closest point to the box is found.
Then a "sensible" second point is found if the angle
between the segment and the box is low enough < 45
int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum margin)
{
mjGETINFO
In the comments that follow, capsule just means the capsule's line segment
It might be hard to understand all comments but you would need
a picture to see what is happening at each line of the code
*/
// raw capsule : box
int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
mjtNum tmp1[3], tmp2[3], tmp3[3], halfaxis[3], axis[3], dif[3];
mjtNum pos[3]; // position of capsule in box-local frame
@@ -576,7 +580,7 @@ skip:
mju_addTo3(tmp2, pos2);
// collide with
n = _SphereBox(con, margin, tmp2, mat1, size1, pos2, mat2, size2);
n = mjraw_SphereBox(con, margin, tmp2, mat1, size1, pos2, mat2, size2);
if (secondpos > -3) { // secondpos was modified
@@ -584,15 +588,24 @@ skip:
mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation
mju_rotVecMat(tmp2, tmp1, mat2);
mju_addTo3(tmp2, pos2);
n += _SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2);
n += mjraw_SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2);
}
return n;
}
// capsule : box
int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum margin)
{
mjGETINFO
return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
// box : box
int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, mjtNum margin)
{
const mjtNum* pos1 = D->geom_xpos + 3 * g1;
src/engine/engine_collision_convex.c
+260 -13
View File
@@ -15,6 +15,7 @@
#include "engine/engine_collision_convex.h"
#include <math.h>
#include <stddef.h>
#include <ccd/ccd.h>
#include <ccd/vec3.h>
@@ -28,12 +29,29 @@
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
// ccd center function
void mjccd_center(const void *obj, ccd_vec3_t *center) {
const mjtCCD* ccd = (const mjtCCD*)obj;
int g = ccd->geom;
int f = ccd->flex;
int e = ccd->elem;
int v = ccd->vert;
// return geom position
mju_copy3(center->v, ccd->data->geom_xpos + 3*ccd->geom);
if (g>=0) {
mju_copy3(center->v, ccd->data->geom_xpos + 3*g);
}
// return flex element position
else if (e>=0) {
mju_copy3(center->v, ccd->data->flexelem_aabb + 6*(ccd->model->flex_elemadr[f]+e));
}
// return flex vertex position
else {
mju_copy3(center->v, ccd->data->flexvert_xpos + 3*(ccd->model->flex_vertadr[f]+v));
}
}
@@ -45,6 +63,46 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
const mjData* d = ccd->data;
int g = ccd->geom;
//-------------------------- flex element or vertex -----------------------------
if (g<0) {
int f = ccd->flex;
int dim = m->flex_dim[f];
mjtNum *res = vec->v;
const mjtNum *dir = _dir->v;
// flex element
if (ccd->elem>=0) {
int e = ccd->elem;
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
// find element vertex with largest projection along dir
mju_copy3(res, vert+3*edata[0]);
mjtNum best = mju_dot3(res, dir);
for (int i=1; i<=dim; i++) {
mjtNum dot = mju_dot3(vert+3*edata[i], dir);
// better vertex found: assign
if (dot>best) {
best = dot;
mju_copy3(res, vert+3*edata[i]);
}
}
// add radius and margin/2
mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*ccd->margin);
return;
}
// flex vertex
else {
const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + ccd->vert);
mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*ccd->margin);
return;
}
}
//-------------------------- geom -------------------------------------------
float* vertdata;
int ibest, graphadr, numvert, change, locid;
int *vert_edgeadr, *vert_globalid, *edge_localid;
@@ -211,7 +269,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
// find single convex-convex collision, using libccd
static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, mjtNum margin) {
ccd_vec3_t dir, pos;
ccd_real_t depth;
if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
@@ -226,8 +284,10 @@ static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
mju_copy3(con->pos, pos.v);
mju_zero3(con->frame+3);
// fix contact frame normal
mjc_fixNormal(m, d, con, g1, g2);
// both geoms: fix contact frame normal
if (obj1->geom>=0 && obj2->geom>=0) {
mjc_fixNormal(m, d, con, obj1->geom, obj2->geom);
}
return 1;
}
@@ -278,8 +338,8 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
int mjc_Convex(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
ccd_t ccd;
mjtCCD obj1 = {m, d, g1, -1, margin, {1, 0, 0, 0}};
mjtCCD obj2 = {m, d, g2, -1, margin, {1, 0, 0, 0}};
mjtCCD obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
mjtCCD obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
// init ccd structure
ccd.first_dir = ccdFirstDirDefault;
@@ -288,12 +348,12 @@ int mjc_Convex(const mjModel* m, const mjData* d,
ccd.support1 = mjccd_support;
ccd.support2 = mjccd_support;
// set ccd paramters
// set ccd parameters
ccd.max_iterations = m->opt.mpr_iterations;
ccd.mpr_tolerance = m->opt.mpr_tolerance;
// find initial contact
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, g1, g2, margin);
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);
// look for additional contacts
if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?)
@@ -342,7 +402,7 @@ int mjc_Convex(const mjModel* m, const mjData* d,
mju_rotateFrame(con[0].pos, invrot, d->geom_xmat+9*g2, d->geom_xpos+3*g2);
// search for new contact
int new_contact = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con+ncon, g1, g2, margin);
int new_contact = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin);
// check new contact
if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) {
@@ -410,7 +470,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
mjGETINFO
mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
ccd_vec3_t dir, vec;
mjtCCD obj = {m, d, g2, -1, 0, {1, 0, 0, 0}};
mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
// get support point in -normal direction
ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]);
@@ -584,14 +644,14 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
int hid = m->geom_dataid[g1];
int nrow = m->hfield_nrow[hid];
int ncol = m->hfield_ncol[hid];
int dr[2], cnt, rmin, rmax, cmin, cmax, nvert;
int dr[2], cnt, rmin, rmax, cmin, cmax;
const float* data = m->hfield_data + m->hfield_adr[hid];
mjtPrism prism;
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjtCCD obj = {m, d, g2, -1, 0, {1, 0, 0, 0}};
mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
ccd_t ccd;
// point size1 to hfield size instead of geom1 size
@@ -711,7 +771,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
// process all prisms in sub-grid
cnt = 0;
for (int r=rmin; r < rmax; r++) {
nvert = 0;
int nvert = 0;
for (int c=cmin; c <= cmax; c++) {
for (int i=0; i < 2; i++) {
// send vertex to prism constructor
@@ -1018,3 +1078,190 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
mju_zero3(con->frame+3);
}
}
//---------------------------- flex collisions ---------------------------------------------
// geom-elem or elem-elem or vert-elem convex collision using ccd
int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
ccd_t ccd;
mjtCCD obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}};
mjtCCD obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}};
// init ccd structure
ccd.first_dir = ccdFirstDirDefault;
ccd.center1 = mjccd_center;
ccd.center2 = mjccd_center;
ccd.support1 = mjccd_support;
ccd.support2 = mjccd_support;
// set ccd parameters
ccd.max_iterations = m->opt.mpr_iterations;
ccd.mpr_tolerance = m->opt.mpr_tolerance;
// find contacts
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);
return ncon;
}
// test a heighfield geom and a flex flex element for collision
int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
int g, int f, int e, mjtNum margin) {
mjtNum vec[3], dx, dy;
mjtNum xmin, xmax, ymin, ymax, zmin, zmax;
int dr[2], cnt, rmin, rmax, cmin, cmax;
mjtPrism prism;
// get hfield info
int hid = m->geom_dataid[g];
int nrow = m->hfield_nrow[hid];
int ncol = m->hfield_ncol[hid];
mjtNum* hpos = d->geom_xpos + 3*g;
mjtNum* hmat = d->geom_xmat + 9*g;
mjtNum* hsize = m->hfield_size + 4*hid;
const float* hdata = m->hfield_data + m->hfield_adr[hid];
// get elem indo
int dim = m->flex_dim[f];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
mjtNum* evert[4] = {NULL, NULL, NULL, NULL};
for (int i=0; i<=dim; i++) {
evert[i] = d->flexvert_xpos + 3*(m->flex_vertadr[f] + edata[i]);
}
mjtNum* ecenter = d->flexelem_aabb + 6*(m->flex_elemadr[f]+e);
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjtCCD obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}};
ccd_t ccd;
//------------------------------------- AABB computation, box-box test
// save elem vertices, transform to hfield frame
mjtNum savevert[4][3];
for (int i=0; i<=dim; i++) {
mju_copy3(savevert[i], evert[i]);
mju_sub3(vec, evert[i], hpos);
mju_mulMatTVec(evert[i], hmat, vec, 3, 3);
}
// save elem center, transform to hfield frame
mjtNum savecenter[3];
mju_copy3(savecenter, ecenter);
mju_sub3(vec, ecenter, hpos);
mju_mulMatTVec(ecenter, hmat, vec, 3, 3);
// compute elem bounding box (in hfield frame)
xmin = xmax = evert[0][0];
ymin = ymax = evert[0][1];
zmin = zmax = evert[0][2];
for (int i=1; i<=dim; i++) {
xmin = mju_min(xmin, evert[i][0]);
xmax = mju_max(xmax, evert[i][0]);
ymin = mju_min(ymin, evert[i][1]);
ymax = mju_max(ymax, evert[i][1]);
zmin = mju_min(zmin, evert[i][2]);
zmax = mju_max(zmax, evert[i][2]);
}
// box-box test
if ((xmin-margin > hsize[0]) || (xmax+margin < -hsize[0]) ||
(ymin-margin > hsize[1]) || (ymax+margin < -hsize[1]) ||
(zmin-margin > hsize[2]) || (zmax+margin < -hsize[3])) {
// restore vertices and center
for (int i=0; i<=dim; i++) {
mju_copy3(evert[i], savevert[i]);
}
mju_copy3(ecenter, savecenter);
return 0;
}
// compute sub-grid bounds
cmin = (int) floor((xmin + hsize[0]) / (2*hsize[0]) * (ncol-1));
cmax = (int) ceil ((xmax + hsize[0]) / (2*hsize[0]) * (ncol-1));
rmin = (int) floor((ymin + hsize[1]) / (2*hsize[1]) * (nrow-1));
rmax = (int) ceil ((ymax + hsize[1]) / (2*hsize[1]) * (nrow-1));
cmin = mjMAX(0, cmin);
cmax = mjMIN(ncol-1, cmax);
rmin = mjMAX(0, rmin);
rmax = mjMIN(nrow-1, rmax);
//------------------------------------- collision testing
// init ccd structure
ccd.first_dir = prism_firstdir;
ccd.center1 = prism_center;
ccd.center2 = mjccd_center;
ccd.support1 = prism_support;
ccd.support2 = mjccd_support;
// set ccd parameters
ccd.max_iterations = m->opt.mpr_iterations;
ccd.mpr_tolerance = m->opt.mpr_tolerance;
// compute real-valued grid step, and triangulation direction
dx = (2.0*hsize[0]) / (ncol-1);
dy = (2.0*hsize[1]) / (nrow-1);
dr[0] = 1;
dr[1] = 0;
// set zbottom value using base size
prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -hsize[3];
// process all prisms in sub-grid
cnt = 0;
for (int r=rmin; r<rmax; r++) {
int nvert = 0;
for (int c=cmin; c<=cmax; c++) {
for (int k=0; k<2; k++) {
// send vertex to prism constructor
addVert(&nvert, &prism, dx*c-hsize[0], dy*(r+dr[k])-hsize[1],
hdata[(r+dr[k])*ncol+c]*hsize[2]+margin);
// check for enough vertices
if (nvert>2) {
// prism height test
if (prism.v[3][2]<zmin && prism.v[4][2]<zmin && prism.v[5][2]<zmin) {
continue;
}
// run MPR, save contact
if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd)==0) {
if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
mju_rotVecMat(con[cnt].frame, dirccd.v, hmat);
mju_rotVecMat(con[cnt].pos, vecccd.v, hmat);
mju_addTo3(con[cnt].pos, hpos);
mju_zero3(con[cnt].frame+3);
// count, stop if max number reached
cnt++;
if (cnt>=mjMAXCONPAIR) {
r = rmax+1;
c = cmax+1;
k = 3;
break;
}
}
}
}
}
}
}
// restore elem vertices and center
for (int i=0; i<=dim; i++) {
mju_copy3(evert[i], savevert[i]);
}
mju_copy3(ecenter, savecenter);
return cnt;
}
src/engine/engine_collision_convex.h
+11 -1
View File
@@ -44,6 +44,9 @@ struct _mjtCCD {
const mjData* data;
int geom;
int meshindex;
int flex;
int elem;
int vert;
mjtNum margin;
mjtNum rotate[4];
};
@@ -54,7 +57,7 @@ typedef struct _mjtCCD mjtCCD;
void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec);
// pairwise collision functions using ccd
// pairwise geom collision functions using ccd
int mjc_PlaneConvex (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
int mjc_ConvexHField (const mjModel* m, const mjData* d,
@@ -62,6 +65,13 @@ int mjc_ConvexHField (const mjModel* m, const mjData* d,
int mjc_Convex (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
// geom-elem or elem-elem or vert-elem collision function using ccd
int mjc_ConvexElem (const mjModel* m, const mjData* d, mjContact* con,
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin);
// heighfield-elem collision function using ccd
int mjc_HFieldElem (const mjModel* m, const mjData* d, mjContact* con,
int g, int f, int e, mjtNum margin);
// fix contact frame normal
void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2);
src/engine/engine_collision_driver.c
+1606 -648
View File
File diff suppressed because it is too large Load Diff
src/engine/engine_collision_driver.h
+35 -12
View File
@@ -23,12 +23,6 @@
extern "C" {
#endif
struct mjCollisionTree_ {
int node1;
int node2;
};
typedef struct mjCollisionTree_ mjCollisionTree;
// collision function pointers and max contact pairs
MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES];
@@ -38,15 +32,44 @@ MJAPI void mj_collision(const mjModel* m, mjData* d);
// applies Separating Axis Theorem for rotated AABBs
MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
const mjtNum xpos1[3], const mjtNum xmat1[9],
const mjtNum xpos2[3], const mjtNum xmat2[9],
const mjtNum xpos2[3], const mjtNum xmat2[9], mjtNum margin,
mjtNum product[36], mjtNum offset[12], mjtByte* initialize);
// broad phase collision detection; return list of body pairs for narrow phase
int mj_broadphase(const mjModel* m, mjData* d, int* bodypair, int maxpair);
// is element active (for collisions)
MJAPI int mj_isElemActive(const mjModel* m, int f, int e);
// checks if pair is already present in pair_geom and calls narrow phase
void mj_collideGeomPair(const mjModel* m, mjData* d, int g1, int g2, int merged,
int startadr, int pairadr);
// binary search between two bodyflex trees
void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr);
// broad phase collision detection; return list of bodyflex pairs
int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair);
// test two geoms for collision, apply filters, add to contact list
void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2);
// test a plane geom and a flex for collision, add to contact list
void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f);
// test for internal flex collisions, add to contact list
void mj_collideFlexInternal(const mjModel* m, mjData* d, int f);
// test active element self-collisions with SAP
void mj_collideFlexSAP(const mjModel* m, mjData* d, int f);
// test a geom and an elem for collision, add to contact list
void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e);
// test two elems for collision, add to contact list
void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2);
// test element and vertex for collision, add to contact list
void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v);
// number of possible collisions based on filters and geom types
int mj_contactFilter(int contype1, int conaffinity1,
int contype2, int conaffinity2);
#ifdef __cplusplus
}
src/engine/engine_collision_primitive.c
+168 -30
View File
@@ -26,10 +26,10 @@
//--------------------------- plane collisions -----------------------------------------------------
// plane : sphere (actual implementation, can be called with modified parameters)
static int _PlaneSphere(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// raw plane : sphere
static int mjraw_PlaneSphere(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// set normal
con[0].frame[0] = mat1[2];
con[0].frame[1] = mat1[5];
@@ -57,7 +57,7 @@ static int _PlaneSphere(mjContact* con, mjtNum margin,
int mjc_PlaneSphere(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
return _PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
return mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
@@ -74,11 +74,11 @@ int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
// get point 1, do sphere-plane test
mjtNum pos[3];
mju_add3(pos, pos2, segment);
int n1 = _PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2);
int n1 = mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2);
// get point 2, do sphere-plane test
mju_sub3(pos, pos2, segment);
int n2 = _PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2);
int n2 = mjraw_PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2);
// align contact frames with capsule axis
if (n1) {
@@ -249,9 +249,9 @@ int mjc_PlaneBox(const mjModel* m, const mjData* d,
//--------------------------- sphere and capsule collisions ----------------------------------------
// sphere : sphere (actual implementation, can be called with modified parameters)
static int _SphereSphere(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
static int mjraw_SphereSphere(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// check bounding spheres (this is called from other functions)
mjtNum dif[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
mjtNum cdist_sqr = mju_dot3(dif, dif);
@@ -288,16 +288,15 @@ static int _SphereSphere(mjContact* con, mjtNum margin,
int mjc_SphereSphere(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
return _SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
// sphere : capsule
int mjc_SphereCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
// raw sphere : capsule
int mjraw_SphereCapsule(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// get capsule length and axis
mjtNum len = size2[1];
mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
@@ -309,7 +308,16 @@ int mjc_SphereCapsule(const mjModel* m, const mjData* d,
// find nearest point on segment, do sphere-sphere test
mju_scl3(vec, axis, x);
mju_addTo3(vec, pos2);
return _SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size2);
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size2);
}
// sphere : capsule
int mjc_SphereCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
return mjraw_SphereCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
@@ -348,7 +356,7 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d,
// side collision: use sphere-sphere
if (collide_side) {
mju_addTo3(a_proj, pos2);
return _SphereSphere(con, margin, pos1, mat1, size1, a_proj, mat2, size2);
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, a_proj, mat2, size2);
}
// cap collision: use plane-sphere
@@ -367,7 +375,7 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d,
mju_addScl3(pos_cap, pos2, axis, -height);
mat_cap = flipmat;
}
int ncon = _PlaneSphere(con, margin, pos_cap, mat_cap, size2, pos1, mat1, size1);
int ncon = mjraw_PlaneSphere(con, margin, pos_cap, mat_cap, size2, pos1, mat1, size1);
if (ncon) {
// flip frame normal (because mjGEOM_PLANE < mjGEOM_SPHERE < mjGEOM_CYLINDER)
mju_scl3(con->frame, con->frame, -1);
@@ -383,16 +391,15 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d,
// sphere-sphere with point sphere at the corner
mjtNum size_zero[1] = {0};
return _SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size_zero);
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size_zero);
}
// capsule : capsule
int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
// raw capsule : capsule
int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// get capsule axes (scaled) and center difference
mjtNum axis1[3] = {mat1[2] * size1[1], mat1[5] * size1[1], mat1[8] * size1[1]};
mjtNum axis2[3] = {mat2[2] * size2[1], mat2[5] * size2[1], mat2[8] * size2[1]};
@@ -434,7 +441,7 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mju_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2);
return _SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
return mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
}
// parallel axes
@@ -447,14 +454,14 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjtNum vec2[3];
mju_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2);
int n1 = _SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
int n1 = mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
// x1 = -1
mju_sub3(vec1, pos1, axis1);
x2 = mju_clip((v + mb) / mc, -1, 1);
mju_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2);
int n2 = _SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
int n2 = mjraw_SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found
if (n1+n2 >= 2) {
@@ -466,7 +473,7 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjtNum x1 = mju_clip((u - mb) / ma, -1, 1);
mju_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1);
int n3 = _SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
int n3 = mjraw_SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found
if (n1+n2+n3 >= 2) {
@@ -478,8 +485,139 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
x1 = mju_clip((u + mb) / ma, -1, 1);
mju_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1);
int n4 = _SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
int n4 = mjraw_SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
return n1+n2+n3+n4;
}
}
// capsule : capsule
int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
// sign of (signed) area of planar triangle
static mjtNum areaSign(const mjtNum p1[2], const mjtNum p2[2], const mjtNum p3[2]) {
return mju_sign((p1[0]-p3[0])*(p2[1]-p3[1]) - (p2[0]-p3[0])*(p1[1]-p3[1]));
}
// find nearest point to p within line segment (u,v); return distance to p
static mjtNum pointSegment(mjtNum res[2], const mjtNum p[2],
const mjtNum u[2], const mjtNum v[2]) {
// make u the origin
mjtNum uv[2] = {v[0]-u[0], v[1]-u[1]};
mjtNum up[2] = {p[0]-u[0], p[1]-u[1]};
// project: find a s.t. uv is orthogonal to (up-a*uv)
mjtNum a = mju_dot(uv, up, 2) / mju_max(mjMINVAL, mju_dot(uv, uv, 2));
// find nearest point to p, clamp to u or v if a is not in (0,1)
if (a<=0) {
res[0] = u[0];
res[1] = u[1];
} else if (a>=1) {
res[0] = v[0];
res[1] = v[1];
} else {
mju_addScl(res, u, uv, a, 2);
}
// compute distance
return mju_sqrt((res[0]-p[0])*(res[0]-p[0]) + (res[1]-p[1])*(res[1]-p[1]));
}
// sphere : triangle with radius
int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
const mjtNum* s, mjtNum rs,
const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt) {
mjtNum rbound = margin + rs + rt;
mjtNum X[3];
// make t1 the origin: triangle is (O,A,B); sphere center is S
mjtNum S[3] = { s[0]-t1[0], s[1]-t1[1], s[2]-t1[2]};
mjtNum A[3] = {t2[0]-t1[0], t2[1]-t1[1], t2[2]-t1[2]};
mjtNum B[3] = {t3[0]-t1[0], t3[1]-t1[1], t3[2]-t1[2]};
// N is normal to triangle plane
mjtNum N[3];
mju_cross(N, A, B);
mju_normalize3(N);
// dstS is signed distance from S to plane; exit if too large
mjtNum dstS = mju_dot3(N, S);
if (mju_abs(dstS) > rbound) {
return 0;
}
// P is projection of S in triangle plane
mjtNum P[3];
mju_addScl3(P, S, N, -dstS);
// construct orthogonal axes (V1~A, V2) of triangle plane
mjtNum V1[3], V2[3];
mju_copy3(V1, A);
mjtNum lenA = mju_normalize3(V1);
mju_cross(V2, N, A);
mju_normalize3(V2);
// triangle is (o,a,b), sphere center is p
mjtNum o[2] = {0, 0};
mjtNum a[2] = {lenA, 0}; // equals {mju_dot3(V1, A), mju_dot3(V2, A)}
mjtNum b[2] = {mju_dot3(V1, B), mju_dot3(V2, B)};
mjtNum p[2] = {mju_dot3(V1, P), mju_dot3(V2, P)};
// copmuted signs of areas of (p,o,a), (p,a,b), (p,b,o)
mjtNum sign1 = areaSign(p, o, a);
mjtNum sign2 = areaSign(p, a, b);
mjtNum sign3 = areaSign(p, b, o);
// p is inside triangle
if (sign1==sign2 && sign2==sign3) {
// P is nearest point to S within triangle
mju_copy3(X, P);
}
// p is not inside triangle
else {
// find nearest point to p on triangle edges (o,a), (a,b), (b,o)
mjtNum x[3][2], dstx[3];
dstx[0] = pointSegment(x[0], p, o, a);
dstx[1] = pointSegment(x[1], p, a, b);
dstx[2] = pointSegment(x[2], p, b, o);
// select minimum
int best = (dstx[0]<dstx[1] && dstx[0]<dstx[2]) ? 0 : (dstx[1]<dstx[2] ? 1 : 2);
// convert x[best] to 3D
mju_scl3(X, V1, x[best][0]);
mju_addToScl3(X, V2, x[best][1]);
}
// X is now the nearest point to S within the 3D triangle (O,A,B)
// compute contact normal and distance
mjtNum nrm[3] = {X[0]-S[0], X[1]-S[1], X[2]-S[2]};
mjtNum dst = mju_normalize3(nrm);
// exit if too far
if (dst>rbound) {
return 0;
}
// construct contact
con[0].dist = dst - rs - rt;
mju_addScl3(con[0].pos, s, nrm, rs + con[0].dist/2);
mju_copy3(con[0].frame, nrm);
mju_zero3(con[0].frame+3);
return 1;
}
src/engine/engine_collision_primitive.h
+15 -1
View File
@@ -32,6 +32,20 @@
extern "C" {
#endif
// raw collision functions (called by mjc_XXX)
int mjraw_SphereCapsule (mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2);
int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2);
int mjraw_CapsuleBox (mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2);
int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
const mjtNum* s, mjtNum rs,
const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt);
// plane collisions
int mjc_PlaneSphere (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
@@ -52,7 +66,7 @@ int mjc_SphereCylinder (const mjModel* m, const mjData* d,
int mjc_CapsuleCapsule (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
// box collisions: from boxcollisions.c
// box collisions: from engine_collision_box.c
int mjc_CapsuleBox (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
int mjc_SphereBox (const mjModel* m, const mjData* d,
src/engine/engine_collision_sdf.c
+1 -1
View File
@@ -447,7 +447,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
int* faces, int* npoints, int* n0,
const mjSDF* sdf) {
const int bvhadr = m->mesh_bvhadr[m->geom_dataid[g]];
const int* faceid = m->bvh_geomid + bvhadr;
const int* faceid = m->bvh_nodeid + bvhadr;
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
const int* child = m->bvh_child + 2*bvhadr;
mjtByte* visited = d->bvh_active + bvhadr;
src/engine/engine_core_constraint.c
+337 -184
View File
@@ -121,6 +121,21 @@ int mj_isDual(const mjModel* m) {
// assign/clamp contact friction parameters
void mj_assignFriction(const mjModel* m, mjtNum* target, const mjtNum* source) {
if (mjENABLED(mjENBL_OVERRIDE)) {
for (int i=0; i<5; i++) {
target[i] = mju_max(mjMINMU, m->opt.o_friction[i]);
}
} else {
for (int i=0; i<5; i++) {
target[i] = mju_max(mjMINMU, source[i]);
}
}
}
// assign/override contact reference parameters
void mj_assignRef(const mjModel* m, mjtNum* target, const mjtNum* source) {
if (mjENABLED(mjENBL_OVERRIDE)) {
@@ -154,6 +169,46 @@ mjtNum mj_assignMargin(const mjModel* m, mjtNum source) {
// compute element bodies and weights for given contact point, return #bodies
// if v is one of the element vertices, reduce element to fragment
static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, int v,
const mjtNum point[3], int* body, mjtNum* weight) {
// get flex info
int dim = m->flex_dim[f];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
// compute inverse distances from contact point to element vertices
// save body ids, find vertex v in element
int vid = -1;
for (int i=0; i<=dim; i++) {
mjtNum dist = mju_dist3(point, vert+3*edata[i]);
weight[i] = 1.0/(mju_max(mjMINVAL, dist));
body[i] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[i]];
// check if element vertex matches v
if (edata[i]==v) {
vid = i;
}
}
// v found in e: skip and shift remaining
if (vid>=0) {
while (vid<dim) {
weight[vid] = weight[vid+1];
body[vid] = body[vid+1];
vid++;
}
dim--;
}
// normalize weights
mju_normalize(weight, dim+1);
return dim+1;
}
// add contact to d->contact list; return 0 if success; 1 if buffer full
int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) {
// if nconmax is specified and ncon >= nconmax, warn and return error
@@ -186,11 +241,10 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) {
// add #size rows to constraint Jacobian; set pos, margin, frictionloss, type, id
// return 0 if success; 1 if buffer full
int mj_addConstraint(const mjModel* m, mjData* d,
const mjtNum* jac, const mjtNum* pos,
const mjtNum* margin, mjtNum frictionloss,
int size, int type, int id, int NV, const int* chain) {
static void mj_addConstraint(const mjModel* m, mjData* d,
const mjtNum* jac, const mjtNum* pos,
const mjtNum* margin, mjtNum frictionloss,
int size, int type, int id, int NV, const int* chain) {
int empty, nv = m->nv, nefc = d->nefc;
int *nnz = d->efc_J_rownnz, *adr = d->efc_J_rowadr, *ind = d->efc_J_colind;
mjtNum *J = d->efc_J;
@@ -231,7 +285,7 @@ int mj_addConstraint(const mjModel* m, mjData* d,
empty = 0;
} else if (empty) {
// all rows are empty, return early
return 0;
return;
}
// chain required in sparse mode
@@ -257,7 +311,7 @@ int mj_addConstraint(const mjModel* m, mjData* d,
// all rows empty: skip constraint
if (empty) {
return 0;
return;
}
// set constraint pos, margin, frictionloss, type, id
@@ -278,85 +332,6 @@ int mj_addConstraint(const mjModel* m, mjData* d,
} else if (type == mjCNSTR_LIMIT_JOINT || type == mjCNSTR_LIMIT_TENDON) {
d->nl += size;
}
return 0;
}
// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) {
int da1, da2, NV = 0;
// skip fixed bodies
while (b1 && !m->body_dofnum[b1]) {
b1 = m->body_parentid[b1];
}
while (b2 && !m->body_dofnum[b2]) {
b2 = m->body_parentid[b2];
}
// neither body is movable: empty chain
if (b1 == 0 && b2 == 0) {
return 0;
}
// intialize last dof address for each body
da1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1;
da2 = m->body_dofadr[b2] + m->body_dofnum[b2] - 1;
// merge chains
while (da1 >= 0 || da2 >= 0) {
chain[NV] = mjMAX(da1, da2);
if (da1 == chain[NV]) {
da1 = m->dof_parentid[da1];
}
if (da2 == chain[NV]) {
da2 = m->dof_parentid[da2];
}
NV++;
}
// reverse order of chain: make it increasing
for (int i=0; i < NV/2; i++) {
int tmp = chain[i];
chain[i] = chain[NV-i-1];
chain[NV-i-1] = tmp;
}
return NV;
}
// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) {
// swap bodies if wrong order
if (b1 > b2) {
int tmp = b1;
b1 = b2;
b2 = tmp;
}
// init
int n1 = m->body_dofnum[b1], n2 = m->body_dofnum[b2];
// both fixed: nothing to do
if (n1 == 0 && n2 == 0) {
return 0;
}
// copy b1 dofs
for (int i=0; i < n1; i++) {
chain[i] = m->body_dofadr[b1] + i;
}
// copy b2 dofs
for (int i=0; i < n2; i++) {
chain[n1+i] = m->body_dofadr[b2] + i;
}
return (n1+n2);
}
@@ -497,6 +472,7 @@ void mj_mulJacTVec_island(const mjModel* m, const mjData* d, mjtNum* res, const
void mj_instantiateEquality(const mjModel* m, mjData* d) {
int issparse = mj_isSparse(m), nv = m->nv;
int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL;
int flex_edgeadr, flex_edgenum;
mjtNum cpos[6], pos[2][3], ref[2], dif, deriv;
mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3];
mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL;
@@ -684,18 +660,39 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
size = 1;
break;
case mjEQ_FLEX:
flex_edgeadr = m->flex_edgeadr[id[0]];
flex_edgenum = m->flex_edgenum[id[0]];
// add one constraint per edge
for (int e=flex_edgeadr; e<flex_edgeadr+flex_edgenum; e++) {
// position error
cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e];
// add constraint: sparse or dense
if (issparse) {
mj_addConstraint(m, d, d->flexedge_J+d->flexedge_J_rowadr[e], cpos, 0, 0,
1, mjCNSTR_EQUALITY, i,
d->flexedge_J_rownnz[e],
d->flexedge_J_colind+d->flexedge_J_rowadr[e]);
} else {
mj_addConstraint(m, d, d->flexedge_J+e*nv, cpos, 0, 0,
1, mjCNSTR_EQUALITY, i,
0, NULL);
}
}
break;
default: // SHOULD NOT OCCUR
mjERROR("invalid equality constraint type %d", m->eq_type[i]);
}
// add constraint
if (size) {
if (mj_addConstraint(m, d, jac[0], cpos, 0, 0,
size, mjCNSTR_EQUALITY, i,
issparse ? NV : 0,
issparse ? chain : NULL)) {
break;
}
mj_addConstraint(m, d, jac[0], cpos, 0, 0,
size, mjCNSTR_EQUALITY, i,
issparse ? NV : 0,
issparse ? chain : NULL);
}
}
}
@@ -732,12 +729,10 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
}
// add constraint
if (mj_addConstraint(m, d, jac, 0, 0, m->dof_frictionloss[i],
1, mjCNSTR_FRICTION_DOF, i,
issparse ? 1 : 0,
issparse ? &i : NULL)) {
break;
}
mj_addConstraint(m, d, jac, 0, 0, m->dof_frictionloss[i],
1, mjCNSTR_FRICTION_DOF, i,
issparse ? 1 : 0,
issparse ? &i : NULL);
}
}
@@ -746,17 +741,14 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
if (m->tendon_frictionloss[i] > 0) {
int efcadr = d->nefc;
// add constraint
if (mj_addConstraint(m, d, d->ten_J + (issparse ? d->ten_J_rowadr[i] : i*nv),
0, 0, m->tendon_frictionloss[i],
1, mjCNSTR_FRICTION_TENDON, i,
issparse ? d->ten_J_rownnz[i] : 0,
issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL)) {
break;
} else {
// set tendon_efcadr
if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr;
}
mj_addConstraint(m, d, d->ten_J + (issparse ? d->ten_J_rowadr[i] : i*nv),
0, 0, m->tendon_frictionloss[i],
1, mjCNSTR_FRICTION_TENDON, i,
issparse ? d->ten_J_rownnz[i] : 0,
issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL);
// set tendon_efcadr
if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr;
}
}
}
@@ -809,12 +801,10 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
}
// add constraint
if (mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i,
issparse ? 1 : 0,
issparse ? m->jnt_dofadr+i : NULL)) {
break;
}
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i,
issparse ? 1 : 0,
issparse ? m->jnt_dofadr+i : NULL);
}
}
}
@@ -845,10 +835,8 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
mju_scl3(jac, angleAxis, -1);
// add constraint
if (mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i, 3, chain)) {
break;
}
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i, 3, chain);
}
// dense
@@ -858,10 +846,8 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
mju_scl3(jac + m->jnt_dofadr[i], angleAxis, -1);
// add constraint
if (mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i, 0, 0)) {
break;
}
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i, 0, 0);
}
}
}
@@ -891,16 +877,13 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// add constraint
int efcadr = d->nefc;
if (mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_TENDON, i,
issparse ? d->ten_J_rownnz[i] : 0,
issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL)) {
break;
} else {
// set tendon_efcadr
if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr;
}
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_TENDON, i,
issparse ? d->ten_J_rownnz[i] : 0,
issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL);
// set tendon_efcadr
if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr;
}
}
}
@@ -915,9 +898,9 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// frictionless and frictional contacts
void mj_instantiateContact(const mjModel* m, mjData* d) {
int ispyramid = mj_isPyramidal(m), issparse = mj_isSparse(m), ncon = d->ncon;
int dim, b1, b2, NV = m->nv, *chain = NULL;
int dim, NV, nv = m->nv, *chain = NULL;
mjContact* con;
mjtNum cpos[6], cmargin[6], *jac, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r;
mjtNum cpos[6], cmargin[6], *jac, *jacdif, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r;
if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0) {
return;
@@ -926,36 +909,84 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
mj_markStack(d);
// allocate Jacobian
jac = mj_stackAllocNum(d, 6*NV);
jacdifp = mj_stackAllocNum(d, 3*NV);
jacdifr = mj_stackAllocNum(d, 3*NV);
jac1p = mj_stackAllocNum(d, 3*NV);
jac2p = mj_stackAllocNum(d, 3*NV);
jac1r = mj_stackAllocNum(d, 3*NV);
jac2r = mj_stackAllocNum(d, 3*NV);
jac = mj_stackAllocNum(d, 6*nv);
jacdif = mj_stackAllocNum(d, 6*nv);
jacdifp = jacdif;
jacdifr = jacdif + 3*nv;
jac1p = mj_stackAllocNum(d, 3*nv);
jac2p = mj_stackAllocNum(d, 3*nv);
jac1r = mj_stackAllocNum(d, 3*nv);
jac2r = mj_stackAllocNum(d, 3*nv);
if (issparse) {
chain = mj_stackAllocInt(d, NV);
chain = mj_stackAllocInt(d, nv);
}
// find contacts to be included
for (int i=0; i < ncon; i++) {
if (!d->contact[i].exclude) {
// get pointer to this contact, info
// get contact info, safe efc_address
con = d->contact + i;
dim = con->dim;
b1 = m->geom_bodyid[con->geom1];
b2 = m->geom_bodyid[con->geom2];
// save efc_address
con->efc_address = d->nefc;
// compute Jacobian differences
if (dim > 3) {
NV = mj_jacDifPair(m, d, chain, b1, b2, con->pos, con->pos,
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr);
} else {
NV = mj_jacDifPair(m, d, chain, b1, b2, con->pos, con->pos,
jac1p, jac2p, jacdifp, NULL, NULL, NULL);
// special case: single body on each side
if ((con->geom[0]>=0 || con->vert[0]>=0) &&
(con->geom[1]>=0 || con->vert[1]>=0)) {
// get bodies
int bid[2];
for (int side=0; side < 2; side++) {
bid[side] = (con->geom[side]>=0) ?
m->geom_bodyid[con->geom[side]] :
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
}
// compute Jacobian differences
if (dim > 3) {
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr);
} else {
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, NULL, NULL, NULL);
}
}
// general case: flex elements involved
else {
// get bodies and weights
int nb = 0;
int bid[8];
mjtNum bweight[8];
for (int side=0; side<2; side++) {
// geom
if (con->geom[side]>=0) {
bid[nb] = m->geom_bodyid[con->geom[side]];
bweight[nb] = side ? +1 : -1;
nb++;
}
// flex vert
else if (con->vert[side]>=0) {
bid[nb] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bweight[nb] = side ? +1 : -1;
nb++;
}
// flex elem
else {
int nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, bid+nb, bweight+nb);
// negative sign for first side of contact
if (side==0) {
mju_scl(bweight+nb, bweight+nb, -1, nw);
}
nb += nw;
}
}
// combine weighted Jacobians
NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim>3);
}
// skip contact if no DOFs affected
@@ -973,7 +1004,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
// make frictionless contact
if (dim == 1) {
// add constraint (already checked space)
// add constraint
mj_addConstraint(m, d, jac, &(con->dist), &(con->includemargin), 0,
1, mjCNSTR_CONTACT_FRICTIONLESS, i,
issparse ? NV : 0,
@@ -992,7 +1023,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
mju_addScl(jacdifp, jac, jac + k*NV, con->friction[k-1], NV);
mju_addScl(jacdifp + NV, jac, jac + k*NV, -con->friction[k-1], NV);
// add constraint (already checked space)
// add constraint
mj_addConstraint(m, d, jacdifp, cpos, cmargin, 0,
2, mjCNSTR_CONTACT_PYRAMIDAL, i,
issparse ? NV : 0,
@@ -1008,7 +1039,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
cpos[0] = con->dist;
cmargin[0] = con->includemargin;
// add constraint (already checked space)
// add constraint
mj_addConstraint(m, d, jac, cpos, cmargin, 0,
con->dim, mjCNSTR_CONTACT_ELLIPTIC, i,
issparse ? NV : 0,
@@ -1026,15 +1057,21 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
// compute diagApprox
void mj_diagApprox(const mjModel* m, mjData* d) {
int id, dim, b1, b2, weldcnt = 0;
int id, dim, b1, b2, weldcnt = 0, edgecnt = 0;
int nefc = d->nefc;
mjtNum tran, rot, fri, *dA = d->efc_diagApprox;
mjContact* con = NULL;
// loop over all constraints, compute approximate inverse inertia
for (int i=0; i < nefc; i++) {
// get constraint id
id = d->efc_id[i];
// clear edge counter
if (d->efc_type[i]!=mjEQ_FLEX) {
edgecnt = 0;
}
// process according to constraint type
switch ((mjtConstraint) d->efc_type[i]) {
case mjCNSTR_EQUALITY:
@@ -1070,6 +1107,11 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
m->tendon_invweight0[m->eq_obj2id[id]]);
break;
case mjEQ_FLEX:
dA[i] = m->flexedge_invweight0[m->flex_edgeadr[m->eq_obj1id[id]] + edgecnt];
edgecnt++;
break;
default:
mjERROR("unknown constraint type type %d", d->efc_type[i]); // SHOULD NOT OCCUR
}
@@ -1091,14 +1133,41 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
case mjCNSTR_CONTACT_FRICTIONLESS:
case mjCNSTR_CONTACT_PYRAMIDAL:
case mjCNSTR_CONTACT_ELLIPTIC:
// get body ids and dim
b1 = m->geom_bodyid[d->contact[id].geom1];
b2 = m->geom_bodyid[d->contact[id].geom2];
dim = d->contact[id].dim;
// get contact info
con = d->contact + id;
dim = con->dim;
// precompute translational and rotational components
tran = m->body_invweight0[2*b1] + m->body_invweight0[2*b2];
rot = m->body_invweight0[2*b1+1] + m->body_invweight0[2*b2+1];
// add the average translation and rotation components from both sides
tran = rot = 0;
for (int side=0; side<2; side++) {
// get bodies and weights
int nb, bid[4];
mjtNum bweight[4];
// geom
if (con->geom[side]>=0) {
bid[0] = m->geom_bodyid[con->geom[side]];
bweight[0] = 1;
nb = 1;
// flex vert
} else if (con->vert[side]>=0) {
bid[0] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bweight[0] = 1;
nb = 1;
// flex elem
} else {
nb = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, bid, bweight);
}
// add weighted average over bodies
for (int k=0; k<nb; k++) {
tran += m->body_invweight0[2*bid[k]] * bweight[k];
rot += m->body_invweight0[2*bid[k]+1] * bweight[k];
}
}
// set frictionless
if (d->efc_type[i] == mjCNSTR_CONTACT_FRICTIONLESS) {
@@ -1118,8 +1187,8 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
// set pyramidal
else {
for (int j=0; j < dim-1; j++) {
fri = d->contact[id].friction[j];
dA[i+2*j] = dA[i+2*j+1] = tran + fri*fri*(j < 2 ? tran : rot);
fri = con->friction[j];
dA[i+2*j] = dA[i+2*j+1] = tran + fri*fri*(j<2 ? tran : rot);
}
// processed 2*dim-2 elements in one i-loop iteration; advance counter
@@ -1444,6 +1513,39 @@ static int mj_jacDifPairCount(const mjModel* m, int* chain,
// count the non-zero columns of the Jacobian returned by mj_jacSum
static int mj_jacSumCount(const mjModel* m, mjData* d, int* chain,
int n, const int* body) {
int nv = m->nv, NV;
mj_markStack(d);
int* bodychain = mj_stackAllocInt(d, nv);
int* tempchain = mj_stackAllocInt(d, nv);
// set first
NV = mj_bodyChain(m, body[0], chain);
// accumulate remaining
for (int i=1; i<n; i++) {
// get body chain
int bodyNV = mj_bodyChain(m, body[i], bodychain);
if (!bodyNV) {
continue;
}
// accumulate chains
NV = mju_addChains(tempchain, nv, NV, bodyNV, chain, bodychain);
if (NV) {
mju_copyInt(chain, tempchain, NV);
}
}
mj_freeStack(d);
return NV;
}
// return number of constraint non-zeros, handle dense and dof-less cases
static inline int mj_addConstraintCount(const mjModel* m, int size, int NV) {
// over count for dense allocation
@@ -1456,11 +1558,12 @@ static inline int mj_addConstraintCount(const mjModel* m, int size, int NV) {
// count equality constraints, count Jacobian nonzeros if nnz is not NULL
static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
int ne = 0, nnze = 0;
int nv = m->nv, neq = m->neq;
int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL;
int issparse = (nnz != NULL);
int flex_edgeadr, flex_edgenum;
// disabled or no equality constraints: return
if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) {
@@ -1535,12 +1638,34 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
NV = 2;
}
break;
case mjEQ_FLEX:
size = m->flex_edgenum[id[0]];
if (!nnz) {
break;
}
flex_edgeadr = m->flex_edgeadr[id[0]];
flex_edgenum = m->flex_edgenum[id[0]];
// process edges of this flex
for (int e=flex_edgeadr; e<flex_edgeadr+flex_edgenum; e++) {
int b1 = m->flex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e+1]];
// accumulate NV
NV += mj_jacDifPairCount(m, chain, b1, b2, issparse);
}
break;
default:
// might occur in case of the now-removed distance equality constraint
mjERROR("unknown constraint type type %d", m->eq_type[i]); // SHOULD NOT OCCUR
}
// accumulate counts; flex NV already accumulated
ne += mj_addConstraintCount(m, size, NV);
nnze += size*NV;
nnze += (m->eq_type[i]==mjEQ_FLEX) ? NV : size*NV;
}
}
@@ -1555,7 +1680,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
// count frictional constraints, count Jacobian nonzeros if nnz is not NULL
static inline int mj_nf(const mjModel* m, const mjData* d, int *nnz) {
static int mj_nf(const mjModel* m, const mjData* d, int *nnz) {
int nf = 0, nnzf = 0;
int nv = m->nv, ntendon = m->ntendon;
@@ -1587,7 +1712,7 @@ static inline int mj_nf(const mjModel* m, const mjData* d, int *nnz) {
// count limit constraints, count Jacobian nonzeros if nnz is not NULL
static inline int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
static int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
int nnzl = 0, nl = 0;
int ntendon = m->ntendon;
int side;
@@ -1654,10 +1779,9 @@ static inline int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
// count contact constraints, count Jacobian nonzeros if nnz is not NULL
static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) {
static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int nnzc = 0, nc = 0;
int ispyramid = mj_isPyramidal(m), ncon = d->ncon;
int issparse = (nnz != NULL);
if (mjDISABLED(mjDSBL_CONTACT) || !ncon) {
return 0;
@@ -1667,19 +1791,49 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int *chain = mj_stackAllocInt(d, m->nv);
for (int i=0; i < ncon; i++) {
if (d->contact[i].exclude) {
continue;
}
mjContact* con = d->contact + i;
int dim = con->dim;
int b1 = m->geom_bodyid[con->geom1];
int b2 = m->geom_bodyid[con->geom2];
int NV = mj_jacDifPairCount(m, chain, b1, b2, issparse);
if (!NV) {
// skip if excluded
if (con->exclude) {
continue;
}
// compute NV only if nnz requested
int NV = 0;
if (nnz) {
// get bodies
int nb = 0, bid[8];
for (int side=0; side < 2; side++) {
// geom
if (con->geom[side]>=0) {
bid[nb++] = m->geom_bodyid[con->geom[side]];
}
// flex vert
else if (con->vert[side] >= 0) {
bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
}
// flex elem
else {
int f = con->flex[side];
int fdim = m->flex_dim[f];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + con->elem[side]*(fdim+1);
for (int k=0; k<=fdim; k++) {
bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[k]];
}
}
}
// count non-zeros in merged chain
NV = mj_jacSumCount(m, d, chain, nb, bid);
if (!NV) {
continue;
}
}
// count according to friction type
int dim = con->dim;
if (dim == 1) {
nc++;
nnzc += NV;
@@ -1743,7 +1897,6 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
mj_instantiateLimit(m, d);
mj_instantiateContact(m, d);
// check sparse allocation
if (mj_isSparse(m)) {
if (d->ne != ne_allocated) {
src/engine/engine_core_constraint.h
+4 -14
View File
@@ -57,25 +57,15 @@ void mj_assignRef(const mjModel* m, mjtNum* target, const mjtNum* source);
// assign/override solver impedance parameters
void mj_assignImp(const mjModel* m, mjtNum* target, const mjtNum* source);
// assign/override geom/limit/tendon margin
// assign/clamp contact friction parameters
void mj_assignFriction(const mjModel* m, mjtNum* target, const mjtNum* source);
// assign/override geom margin
mjtNum mj_assignMargin(const mjModel* m, mjtNum source);
// add contact to d->contact list; return 0 if success; 1 if buffer full
MJAPI int mj_addContact(const mjModel* m, mjData* d, const mjContact* con);
// add #size rows to constraint Jacobian; set pos, margin, frictionloss, type, id
// result: 0=success; 1=buffer full
int mj_addConstraint(const mjModel* m, mjData* d,
const mjtNum* jac, const mjtNum* pos,
const mjtNum* margin, mjtNum frictionloss,
int size, int type, int id, int NV, const int* chain);
// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2);
// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2);
//-------------------------- constraint instantiation ----------------------------------------------
src/engine/engine_core_smooth.c
+236 -4
View File
@@ -386,6 +386,221 @@ void mj_camlight(const mjModel* m, mjData* d) {
// update dynamic BVH; leaf aabbs must be updated before call
void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) {
mj_markStack(d);
int* modified = mj_stackAllocInt(d, bvhnum);
mju_zeroInt(modified, bvhnum);
// mark leafs as modified
for (int i=0; i<bvhnum; i++) {
if (m->bvh_nodeid[bvhadr+i]>=0) {
modified[i] = 1;
}
}
// update non-leafs in backward pass (parents come before children)
for (int i=bvhnum-1; i>=0; i--) {
if (m->bvh_nodeid[bvhadr+i]<0) {
int child1 = m->bvh_child[2*(bvhadr+i)];
int child2 = m->bvh_child[2*(bvhadr+i)+1];
// update if either child is modified
if (modified[child1] || modified[child2]) {
mjtNum* aabb = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic + i);
const mjtNum* aabb1 = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic + child1);
const mjtNum* aabb2 = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic + child2);
// compute new (min, max)
mjtNum xmin[3], xmax[3];
for (int k=0; k<3; k++) {
xmin[k] = mju_min(aabb1[k] - aabb1[k+3], aabb2[k] - aabb2[k+3]);
xmax[k] = mju_max(aabb1[k] + aabb1[k+3], aabb2[k] + aabb2[k+3]);
}
// convert to (center, size)
for (int k=0; k<3; k++) {
aabb[k] = 0.5*(xmax[k]+xmin[k]);
aabb[k+3] = 0.5*(xmax[k]-xmin[k]);
}
modified[i] = 1;
}
}
}
mj_freeStack(d);
}
// compute flex-related quantities
void mj_flex(const mjModel* m, mjData* d) {
int nv = m->nv, issparse = mj_isSparse(m);
int* rowadr = d->flexedge_J_rowadr, *rownnz = d->flexedge_J_rownnz;
mjtNum* J = d->flexedge_J;
// skip if no flexes
if (!m->nflex) {
return;
}
// compute Cartesian positions of flex vertices
for (int f=0; f<m->nflex; f++) {
int vstart = m->flex_vertadr[f];
int vend = m->flex_vertadr[f] + m->flex_vertnum[f];
// centered: copy body position
if (m->flex_centered[f]) {
for (int i=vstart; i<vend; i++) {
mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
// non-centered: map from local to global
else {
for (int i=vstart; i<vend; i++) {
mju_rotVecMat(d->flexvert_xpos+3*i, m->flex_vert+3*i, d->xmat+9*m->flex_vertbodyid[i]);
mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
}
// compute flex element aabb
for (int f=0; f<m->nflex; f++) {
int dim = m->flex_dim[f];
// process elements of this flex
for (int e=0; e<m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
// compute min and max along each global axis
mjtNum xmin[3], xmax[3];
mju_copy3(xmin, vert+3*edata[0]);
mju_copy3(xmax, vert+3*edata[0]);
for (int i=1; i<=dim; i++) {
for (int j=0; j<3; j++) {
mjtNum value = vert[3*edata[i]+j];
xmin[j] = mju_min(xmin[j], value);
xmax[j] = mju_max(xmax[j], value);
}
}
// compute aabb (center, size)
int base = m->flex_elemadr[f] + e;
d->flexelem_aabb[6*base+0] = 0.5*(xmax[0]+xmin[0]);
d->flexelem_aabb[6*base+1] = 0.5*(xmax[1]+xmin[1]);
d->flexelem_aabb[6*base+2] = 0.5*(xmax[2]+xmin[2]);
d->flexelem_aabb[6*base+3] = 0.5*(xmax[0]-xmin[0]) + m->flex_radius[f];
d->flexelem_aabb[6*base+4] = 0.5*(xmax[1]-xmin[1]) + m->flex_radius[f];
d->flexelem_aabb[6*base+5] = 0.5*(xmax[2]-xmin[2]) + m->flex_radius[f];
}
}
// update flex bhv_aabb_dyn if needed
if (!mjDISABLED(mjDSBL_MIDPHASE)) {
for (int f=0; f<m->nflex; f++) {
if (m->flex_bvhadr[f]>=0) {
int flex_bvhadr = m->flex_bvhadr[f];
int flex_bvhnum = m->flex_bvhnum[f];
// copy element aabbs to bhv leaf aabbs
for (int i=flex_bvhadr; i<flex_bvhadr+flex_bvhnum; i++) {
if (m->bvh_nodeid[i]>=0) {
mju_copy(d->bvh_aabb_dyn + 6*(i - m->nbvhstatic),
d->flexelem_aabb + 6*(m->flex_elemadr[f] + m->bvh_nodeid[i]), 6);
}
}
// update dynamic BVH
mj_updateDynamicBVH(m, d, m->flex_bvhadr[f], m->flex_bvhnum[f]);
}
}
}
// allocate space
mj_markStack(d);
mjtNum* jac1 = mj_stackAllocNum(d, 3*nv);
mjtNum* jac2 = mj_stackAllocNum(d, 3*nv);
mjtNum* jacdif = mj_stackAllocNum(d, 3*nv);
int* chain = issparse ? mj_stackAllocInt(d, nv) : NULL;
// clear Jacobian: sparse or dense
if (issparse) {
mju_zeroInt(rowadr, m->nflexedge);
mju_zeroInt(rownnz, m->nflexedge);
} else {
mju_zero(J, m->nflexedge*nv);
}
// compute lengths and Jacobians of edges
for (int f=0; f<m->nflex; f++) {
// skip if edges cannot generate forces
if (m->flex_rigid[f] ||
(m->flex_edgeequality[f]==0 &&
m->flex_edgestiffness[f]==0 && m->flex_edgedamping[f]==0)) {
continue;
}
// process edges of this flex
int vbase = m->flex_vertadr[f];
int ebase = m->flex_edgeadr[f];
for (int e=0; e<m->flex_edgenum[f]; e++) {
int v1 = m->flex_edge[2*(ebase+e)];
int v2 = m->flex_edge[2*(ebase+e)+1];
int b1 = m->flex_vertbodyid[vbase+v1];
int b2 = m->flex_vertbodyid[vbase+v2];
mjtNum* pos1 = d->flexvert_xpos + 3*(vbase+v1);
mjtNum* pos2 = d->flexvert_xpos + 3*(vbase+v2);
// vec = unit vector from v1 to v2, compute edge length
mjtNum vec[3];
mju_sub3(vec, pos2, pos1);
d->flexedge_length[ebase+e] = mju_normalize3(vec);
// sparse edge Jacobian
if (issparse) {
// set rowadr
if (ebase+e>0) {
rowadr[ebase+e] = rowadr[ebase+e-1] + rownnz[ebase+e-1];
}
// get endpoint Jacobians, subtract
int NV = mj_jacDifPair(m, d, chain, b1, b2, pos1, pos2,
jac1, jac2, jacdif, NULL, NULL, NULL);
// no dofs: skip
if (!NV) {
continue;
}
// apply chain rule to compute edge Jacobian
mju_mulMatTVec(J + rowadr[ebase+e], jacdif, vec, 3, NV);
// copy sparsity info
rownnz[ebase+e] = NV;
mju_copyInt(d->flexedge_J_colind + rowadr[ebase+e], chain, NV);
}
// dense edge Jacobian
else {
// get endpoint Jacobians, subtract
mj_jac(m, d, jac1, NULL, pos1, b1);
mj_jac(m, d, jac2, NULL, pos2, b2);
mju_sub(jacdif, jac2, jac1, 3*nv);
// apply chain rule to compute edge Jacobian
mju_mulMatTVec(J + (ebase+e)*nv, jacdif, vec, 3, nv);
}
}
}
mj_freeStack(d);
}
// compute tendon lengths and moments
void mj_tendon(const mjModel* m, mjData* d) {
int issparse = mj_isSparse(m), nv = m->nv, nten = m->ntendon;
@@ -893,8 +1108,15 @@ void mj_transmission(const mjModel* m, mjData* d) {
int counter = 0;
for (int j=0; j < d->ncon; j++) {
const mjContact* con = d->contact+j;
int b1 = m->geom_bodyid[con->geom1];
int b2 = m->geom_bodyid[con->geom2];
// contact involving flex, continue
if (con->geom[0]<0 || con->geom[1]<0) {
continue;
}
// get body ids
int b1 = m->geom_bodyid[con->geom[0]];
int b2 = m->geom_bodyid[con->geom[1]];
// irrelevant contact, continue
if (b1 != id && b2 != id) {
@@ -1549,6 +1771,11 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// get contact pointer
con = d->contact+i;
// skip contact involving flex
if (con->geom[0]<0 || con->geom[1]<0) {
continue;
}
// tmp = contact-local force:torque vector
mj_contactForce(m, d, i, lfrc);
@@ -1558,7 +1785,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 1
int k;
if ((k = m->geom_bodyid[con->geom1])) {
if ((k = m->geom_bodyid[con->geom[0]])) {
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0);
@@ -1567,7 +1794,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
// body 2
if ((k = m->geom_bodyid[con->geom2])) {
if ((k = m->geom_bodyid[con->geom[1]])) {
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0);
@@ -1631,6 +1858,11 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
i++;
break;
case mjEQ_FLEX:
// increment edgenum rows
i += m->flex_edgenum[m->eq_obj1id[id]];
break;
default:
mjERROR("unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR
}
src/engine/engine_core_smooth.h
+3
View File
@@ -33,6 +33,9 @@ MJAPI void mj_comPos(const mjModel* m, mjData* d);
// compute camera and light positions and orientations
MJAPI void mj_camlight(const mjModel* m, mjData* d);
// compute flex-related quantities
MJAPI void mj_flex(const mjModel* m, mjData* d);
// compute tendon lengths, velocities and moment arms
MJAPI void mj_tendon(const mjModel* m, mjData* d);
src/engine/engine_derivative.c
+44 -73
View File
@@ -25,6 +25,7 @@
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
#include "engine/engine_util_sparse.h"
@@ -693,41 +694,6 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
//--------------------- utility functions for (d force / d vel) Jacobians --------------------------
// construct sparse Jacobian structure of body; return nnz
static int bodyJacSparse(const mjModel* m, int body, int* ind) {
// skip fixed bodies
while (body > 0 && m->body_dofnum[body] == 0) {
body = m->body_parentid[body];
}
// body is not movable: empty chain
if (body == 0) {
return 0;
}
// count dofs
int nnz = 0;
int dof = m->body_dofadr[body] + m->body_dofnum[body] - 1;
while (dof >= 0) {
nnz++;
dof = m->dof_parentid[dof];
}
// fill array in reverse (increasing dof)
int cnt = 0;
dof = m->body_dofadr[body] + m->body_dofnum[body] - 1;
while (dof >= 0) {
ind[nnz-cnt-1] = dof;
cnt++;
dof = m->dof_parentid[dof];
}
return nnz;
}
// add J'*B*J to qDeriv
static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* B, int n) {
int nv = m->nv;
@@ -778,30 +744,23 @@ static void addJTBJSparse(
// compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) )
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
int offset_i = offset+i, offset_j = offset+j;
if (!B[i*n+j]) {
continue;
}
mju_zero(row, nv);
// loop over non-zero elements of J(i,:)
for (int k = 0; k < J_rownnz[offset+i]; k++) {
int ik = J_rowadr[offset+i] + k;
mjtNum scl = J[ik]*B[i*n+j];
for (int k = 0; k < J_rownnz[offset_i]; k++) {
int ik = J_rowadr[offset_i] + k;
int colik = J_colind[ik];
// loop over non-zero elements of J(j,:)
// (pJ is the sparse column index into J)
for (int pJ = 0; pJ < J_rownnz[offset+j]; pJ++) {
int adr = J_rowadr[offset+j] + pJ;
row[J_colind[adr]] = scl * J[adr];
}
// row = J(i,k)*B(i,j)*J(j,:)
mju_scl(row, J + J_rowadr[offset_j], J[ik]*B[i*n+j], J_rownnz[offset_j]);
// add row to qDeriv(k,:)
// (pD is the sparse column index into qDeriv)
for (int pD = 0; pD < d->D_rownnz[k]; pD++) {
int adr = d->D_rowadr[k] + pD;
d->qDeriv[adr] += row[d->D_colind[adr]];
}
// qDeriv(k,:) += row
mju_addToSparseInc(d->qDeriv + d->D_rowadr[colik], row,
d->D_rownnz[colik], d->D_colind + d->D_rowadr[colik],
J_rownnz[offset_j], J_colind + J_rowadr[offset_j]);
}
}
}
@@ -1242,7 +1201,7 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
if (mj_isSparse(m)) {
// get sparse body Jacobian structure
nnz = bodyJacSparse(m, bodyid, colind);
nnz = mj_bodyChain(m, bodyid, colind);
// prepare rownnz, rowadr, colind for all 6 rows
for (int i=0; i < 6; i++) {
@@ -1282,16 +1241,11 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
// subtract translational component from grom velocity
mju_subFrom3(lvel+3, lwind+3);
// get body global Jacobian: rotation then translation
mj_jacGeom(m, d, J+3*nv, J, geomid);
// compress geom Jacobian in-place
// get geom global Jacobian: rotation then translation
if (mj_isSparse(m)) {
for (int i=0; i < 6; i++) {
for (int k=0; k < nnz; k++) {
J[i*nnz+k] = J[i*nv+colind[k]];
}
}
mj_jacSparse(m, d, J+3*nnz, J, d->geom_xpos+3*geomid, m->geom_bodyid[geomid], nnz, colind);
} else {
mj_jacGeom(m, d, J+3*nv, J, geomid);
}
// rotate (compressed) Jacobian to local frame
@@ -1334,6 +1288,7 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
}
// fluid forces based on inertia-box approximation
void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
{
@@ -1368,23 +1323,16 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
// subtract translational component from body velocity
mju_subFrom3(lvel+3, lwind+3);
// get body global Jacobian: rotation then translation
mj_jacBodyCom(m, d, J+3*nv, J, i);
// init with dense
int nnz = nv;
// prepare for sparse
// sparse Jacobian
if (mj_isSparse(m)) {
// get sparse body Jacobian structure
nnz = bodyJacSparse(m, i, colind);
nnz = mj_bodyChain(m, i, colind);
// compress body Jacobian in-place
for (int j=0; j < 6; j++) {
for (int k=0; k < nnz; k++) {
J[j*nnz+k] = J[j*nv+colind[k]];
}
}
// get sparse jacBodyCom
mj_jacSparse(m, d, J+3*nnz, J, d->xipos+3*i, i, nnz, colind);
// prepare rownnz, rowadr, colind for all 6 rows
rownnz[0] = nnz;
@@ -1398,6 +1346,11 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
}
}
// dense Jacobian
else {
mj_jacBodyCom(m, d, J+3*nv, J, i);
}
// rotate (compressed) Jacobian to local frame
mju_mulMatTMat(tmp, d->ximat+9*i, J, 3, 3, nnz);
mju_copy(J, tmp, 3*nnz);
@@ -1517,6 +1470,24 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
}
}
// flex edge damping
for (int f=0; f<m->nflex; f++) {
if (!m->flex_rigid[f] && m->flex_edgedamping[f]) {
mjtNum B = -m->flex_edgedamping[f];
// process edges of this flex
for (int e=m->flex_edgeadr[f]; e<m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(m, d, d->flexedge_J, &B, 1, e,
d->flexedge_J_rownnz, d->flexedge_J_rowadr, d->flexedge_J_colind);
} else {
addJTBJ(m, d, d->flexedge_J+e*nv, &B, 1);
}
}
}
}
// tendon damping
for (int i=0; i < m->ntendon; i++) {
if (m->tendon_damping[i] > 0) {
src/engine/engine_forward.c
+11 -2
View File
@@ -127,6 +127,7 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
mj_kinematics(m, d);
mj_comPos(m, d);
mj_camlight(m, d);
mj_flex(m, d);
mj_tendon(m, d);
TM_END(mjTIMER_POS_KINEMATICS);
@@ -182,6 +183,14 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
void mj_fwdVelocity(const mjModel* m, mjData* d) {
TM_START;
// flexedge velocity: dense or sparse
if (mj_isSparse(m)) {
mju_mulMatVecSparse(d->flexedge_velocity, d->flexedge_J, d->qvel, m->nflexedge,
d->flexedge_J_rownnz, d->flexedge_J_rowadr, d->flexedge_J_colind, NULL);
} else {
mju_mulMatVec(d->flexedge_velocity, d->flexedge_J, d->qvel, m->nflexedge, m->nv);
}
// tendon velocity: dense or sparse
if (mj_isSparse(m)) {
mju_mulMatVecSparse(d->ten_velocity, d->ten_J, d->qvel, m->ntendon,
@@ -190,10 +199,10 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
mju_mulMatVec(d->ten_velocity, d->ten_J, d->qvel, m->ntendon, m->nv);
}
// actuator velocity
// actuator velocity: always dense
mju_mulMatVec(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu, m->nv);
// standard velocity computations
// com-based velocities, passive forces, constraint references
mj_comVel(m, d);
mj_passive(m, d);
mj_referenceConstraint(m, d);
src/engine/engine_inverse.c
+3 -23
View File
@@ -25,7 +25,7 @@
#include "engine/engine_derivative.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_passive.h"
#include "engine/engine_forward.h"
#include "engine/engine_sensor.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
@@ -40,6 +40,7 @@ void mj_invPosition(const mjModel* m, mjData* d) {
mj_kinematics(m, d);
mj_comPos(m, d);
mj_camlight(m, d);
mj_flex(m, d);
mj_tendon(m, d);
TM_END(mjTIMER_POS_KINEMATICS);
@@ -63,28 +64,7 @@ void mj_invPosition(const mjModel* m, mjData* d) {
// velocity-dependent computations
void mj_invVelocity(const mjModel* m, mjData* d) {
TM_START;
// tendon velocity: dense or sparse
if (mj_isSparse(m)) {
mju_mulMatVecSparse(d->ten_velocity, d->ten_J, d->qvel, m->ntendon,
d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind, NULL);
} else {
mju_mulMatVec(d->ten_velocity, d->ten_J, d->qvel, m->ntendon, m->nv);
}
// actuator velocity
mju_mulMatVec(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu, m->nv);
// standard velocity computations
mj_comVel(m, d);
mj_passive(m, d);
mj_referenceConstraint(m, d);
// compute qfrc_bias with abbreviated RNE (without acceleration)
mj_rne(m, d, 0, d->qfrc_bias);
TM_END(mjTIMER_VELOCITY);
mj_fwdVelocity(m, d);
}
src/engine/engine_io.c
+69 -39
View File
@@ -144,6 +144,11 @@ void mj_defaultOption(mjOption* opt) {
// solver overrides
opt->o_margin = 0;
mj_defaultSolRefImp(opt->o_solref, opt->o_solimp);
opt->o_friction[0] = 1;
opt->o_friction[1] = 1;
opt->o_friction[2] = 0.005;
opt->o_friction[3] = 0.0001;
opt->o_friction[4] = 0.0001;
// discrete options
opt->integrator = mjINT_EULER;
@@ -420,7 +425,7 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer,
#if (__has_builtin(__builtin_add_overflow) && __has_builtin(__builtin_mul_overflow)) \
|| (defined(__GNUC__) && __GNUC__ >= 5)
// supported by GCC and Clang
int to_add = 0;
size_t to_add = 0;
if (__builtin_mul_overflow(nc, nr, &to_add)) return 0;
if (__builtin_mul_overflow(to_add, type_size, &to_add)) return 0;
if (__builtin_add_overflow(to_add, SKIP(*offset), &to_add)) return 0;
@@ -438,18 +443,20 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer,
// allocate and initialize mjModel structure
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int njnt,
int ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata,
int ntex, int ntexdata, int nmat, int npair, int nexclude,
int neq, int ntendon, int nwrap, int nsensor,
int nnumeric, int nnumericdata, int ntext, int ntextdata,
int ntuple, int ntupledata, int nkey, int nmocap, int nplugin,
int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames, int npaths) {
mjModel* mj_makeModel(
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic,
int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam, int nlight,
int nflex, int nflexvert, int nflexedge, int nflexelem, int nflexelemdata,
int nflexshelldata, int nflexevpair, int nflextexcoord, int nmesh,
int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata, int ntex,
int ntexdata, int nmat, int npair, int nexclude, int neq, int ntendon,
int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext,
int ntextdata, int ntuple, int ntupledata, int nkey, int nmocap,
int nplugin, int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames, int npaths) {
intptr_t offset = 0;
// allocate mjModel
@@ -466,11 +473,21 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n
m->na = na;
m->nbody = nbody;
m->nbvh = nbvh;
m->nbvhstatic = nbvhstatic;
m->nbvhdynamic = nbvhdynamic;
m->njnt = njnt;
m->ngeom = ngeom;
m->nsite = nsite;
m->ncam = ncam;
m->nlight = nlight;
m->nflex = nflex;
m->nflexvert = nflexvert;
m->nflexedge = nflexedge;
m->nflexelem = nflexelem;
m->nflexelemdata = nflexelemdata;
m->nflexshelldata = nflexshelldata;
m->nflexevpair = nflexevpair;
m->nflextexcoord = nflextexcoord;
m->nmesh = nmesh;
m->nmeshvert = nmeshvert;
m->nmeshnormal = nmeshnormal;
@@ -514,7 +531,7 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n
m->nuser_sensor = nuser_sensor;
m->nnames = nnames;
m->nnames_map = mjLOAD_MULTIPLE
* (nbody + njnt + ngeom + nsite + ncam + nlight + nmesh
* (nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh
+ nskin + nhfield + ntex + nmat + npair + nexclude + neq
+ ntendon + nu + nsensor + nnumeric + ntext + ntuple
+ nkey + nplugin);
@@ -578,26 +595,28 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n
return m;
}
// copy mjModel, if dest==NULL create new model
mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
void* save_bufptr;
// allocate new model if needed
if (!dest) {
dest = mj_makeModel(src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, src->njnt,
src->ngeom, src->nsite, src->ncam, src->nlight, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata,
src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude,
src->neq, src->ntendon, src->nwrap, src->nsensor,
src->nnumeric, src->nnumericdata, src->ntext, src->ntextdata,
src->ntuple, src->ntupledata, src->nkey, src->nmocap, src->nplugin,
src->npluginattr, src->nuser_body, src->nuser_jnt, src->nuser_geom,
src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->nuser_sensor, src->nnames, src->npaths);
dest = mj_makeModel(
src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge,
src->nflexelem, src->nflexelemdata, src->nflexshelldata,
src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata,
src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude,
src->neq, src->ntendon, src->nwrap, src->nsensor, src->nnumeric,
src->nnumericdata, src->ntext, src->ntextdata, src->ntuple,
src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr,
src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site,
src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->nuser_sensor, src->nnames, src->npaths);
}
if (!dest) {
mjERROR("failed to make mjModel. Invalid sizes.");
@@ -762,7 +781,9 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
ints[28], ints[29], ints[30], ints[31], ints[32], ints[33], ints[34],
ints[35], ints[36], ints[37], ints[38], ints[39], ints[40], ints[41],
ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
ints[49], ints[50], ints[51], ints[52], ints[53]);
ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
ints[63]);
if (!m || m->nbuffer != sizes[getnsize()-1]) {
mju_closeResource(r);
mju_warning("Corrupted model, wrong size parameters");
@@ -1508,12 +1529,6 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
mju_zeroInt(d->solver_nnz, mjNISLAND);
mju_zero(d->solver_fwdinv, 2);
// clear collision diagnostics
d->nbodypair_broad = 0;
d->nbodypair_narrow = 0;
d->ngeompair_mid = 0;
d->ngeompair_narrow = 0;
// clear variable sizes
d->ne = 0;
d->nf = 0;
@@ -1751,6 +1766,8 @@ static int numObjects(const mjModel* m, mjtObj objtype) {
return m->ncam;
case mjOBJ_LIGHT:
return m->nlight;
case mjOBJ_FLEX:
return m->nflex;
case mjOBJ_MESH:
return m->nmesh;
case mjOBJ_SKIN:
@@ -1795,6 +1812,8 @@ const char* mj_validateReferences(const mjModel* m) {
// ntarget: number of elements in array where references are pointing
// numarray: if refarray is an adr array, numarray is the corresponding num array, otherwise 0
// add flex fields (b/303056369)
#define MJMODEL_REFERENCES \
X(body_parentid, nbody, nbody , 0 ) \
X(body_rootid, nbody, nbody , 0 ) \
@@ -1948,11 +1967,11 @@ const char* mj_validateReferences(const mjModel* m) {
}
}
for (int i=0; i < m->npair; i++) {
int pair_body1 = (m->pair_signature[i] & 0xFFFF) - 1;
int pair_body1 = (m->pair_signature[i] & 0xFFFF);
if (pair_body1 >= m->nbody || pair_body1 < 0) {
return "Invalid model: pair_body1 out of bounds.";
}
int pair_body2 = (m->pair_signature[i] >> 16) - 1;
int pair_body2 = (m->pair_signature[i] >> 16);
if (pair_body2 >= m->nbody || pair_body2 < 0) {
return "Invalid model: pair_body2 out of bounds.";
}
@@ -1991,6 +2010,17 @@ const char* mj_validateReferences(const mjModel* m) {
}
break;
case mjEQ_FLEX:
if (obj1id >= m->nflex || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
// -1 is the value used if second object is omitted
if (obj2id != -1) {
return "Invalid model: eq_obj2id must be -1.";
}
break;
default:
// might occur in case of the now-removed distance equality constraint
mjERROR("unknown equality constraint type.");
@@ -2096,11 +2126,11 @@ const char* mj_validateReferences(const mjModel* m) {
}
}
for (int i=0; i < m->nexclude; i++) {
int exclude_body1 = (m->exclude_signature[i] & 0xFFFF) - 1;
int exclude_body1 = (m->exclude_signature[i] & 0xFFFF);
if (exclude_body1 >= m->nbody || exclude_body1 < 0) {
return "Invalid model: exclude_body1 out of bounds.";
}
int exclude_body2 = (m->exclude_signature[i] >> 16) - 1;
int exclude_body2 = (m->exclude_signature[i] >> 16);
if (exclude_body2 >= m->nbody || exclude_body2 < 0) {
return "Invalid model: exclude_body2 out of bounds.";
}
src/engine/engine_io.h
+4 -3
View File
@@ -51,9 +51,10 @@ void mj_defaultStatistic(mjStatistic* stat);
//------------------------------- mjModel ----------------------------------------------------------
// allocate mjModel
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int njnt,
int ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert,
int nflexedge, int nflexelem, int nflexelemdata, int nflexshelldata, int nflexevpair,
int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata,
int ntex, int ntexdata, int nmat, int npair, int nexclude,
src/engine/engine_island.c
+2 -2
View File
@@ -235,8 +235,8 @@ static int treeFirst(const mjModel* m, const mjData* d, int tree[2], int i) {
if (efc_type == mjCNSTR_CONTACT_FRICTIONLESS ||
efc_type == mjCNSTR_CONTACT_PYRAMIDAL ||
efc_type == mjCNSTR_CONTACT_ELLIPTIC) {
tree[0] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom1]];
tree[1] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom2]];
tree[0] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom[0]]];
tree[1] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom[1]]];
// handle static bodies
if (tree[0] < 0) {
src/engine/engine_passive.c
+42 -2
View File
@@ -51,6 +51,11 @@ void mj_passive(const mjModel* m, mjData* d) {
for (int i=0; i < m->njnt; i++) {
stiffness = m->jnt_stiffness[i];
// disabled : nothing to do
if (stiffness==0) {
continue;
}
int padr = m->jnt_qposadr[i];
int dadr = m->jnt_dofadr[i];
@@ -86,8 +91,38 @@ void mj_passive(const mjModel* m, mjData* d) {
// dof-level dampers
for (int i=0; i < m->nv; i++) {
damping = m->dof_damping[i];
d->qfrc_passive[i] -= damping*d->qvel[i];
if ((damping = m->dof_damping[i]) != 0) {
d->qfrc_passive[i] -= damping*d->qvel[i];
}
}
// flexedge-level spring-dampers
for (int f=0; f < m->nflex; f++) {
stiffness = m->flex_edgestiffness[f];
damping = m->flex_edgedamping[f];
// disabled or rigid: nothing to do
if (m->flex_rigid[f] || (stiffness==0 && damping==0)) {
continue;
}
// process edges of this flex (global edge index)
int edgeend = m->flex_edgeadr[f] + m->flex_edgenum[f];
for (int e=m->flex_edgeadr[f]; e < edgeend; e++) {
// compute spring-damper force along edge
frc = stiffness * (m->flexedge_length0[e] - d->flexedge_length[e])
- damping * d->flexedge_velocity[e];
// transform to joint torque, add to qfrc_passive: dense or sparse
if (issparse) {
int end = d->flexedge_J_rowadr[e] + d->flexedge_J_rownnz[e];
for (int j=d->flexedge_J_rowadr[e]; j < end; j++) {
d->qfrc_passive[d->flexedge_J_colind[j]] += d->flexedge_J[j] * frc;
}
} else {
mju_addToScl(d->qfrc_passive, d->flexedge_J+e*nv, frc, nv);
}
}
}
// tendon-level spring-dampers
@@ -95,6 +130,11 @@ void mj_passive(const mjModel* m, mjData* d) {
stiffness = m->tendon_stiffness[i];
damping = m->tendon_damping[i];
// disabled : nothing to do
if (stiffness == 0 && damping == 0) {
continue;
}
// compute spring force along tendon
mjtNum length = d->ten_length[i];
mjtNum lower = m->tendon_lengthspring[2*i];
src/engine/engine_print.c
+37 -14
View File
@@ -361,6 +361,14 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
if (m->nbody) fprintf(fp, "\n");
// BVHs
for (int i=0; i<m->nbvh; i++) {
fprintf(fp, "\nBVH %d:\n", i);
object_class = &m->nbvh;
MJMODEL_POINTERS
}
if (m->nbvh) fprintf(fp, "\n");
// joints
for (int i=0; i < m->njnt; i++) {
fprintf(fp, "\nJOINT %d:\n", i);
@@ -419,6 +427,16 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
if (m->nlight) fprintf(fp, "\n");
// flexes
for (int i=0; i<m->nflex; i++) {
fprintf(fp, "\nFLEX %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_flexadr[i]);
object_class = &m->nflex;
MJMODEL_POINTERS
}
if (m->nflex) fprintf(fp, "\n");
// meshes
for (int i=0; i < m->nmesh; i++) {
fprintf(fp, "\nMESH %d:\n", i);
@@ -914,6 +932,18 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArray("CDOF", m->nv, 6, d->cdof, fp, float_format);
printArray("CINERT", m->nbody, 10, d->cinert, fp, float_format);
printArray("FLEXVERT_XPOS", m->nflexvert, 3, d->flexvert_xpos, fp, float_format);
printArray("FLEXELEM_AABB", m->nflexelem, 6, d->flexelem_aabb, fp, float_format);
if (!mj_isSparse(m)) {
printArray("FLEXEDGE_J", m->nflexedge, m->nv, d->flexedge_J, fp, float_format);
} else {
printArrayInt("FLEXEDGE_J_ROWNNZ", m->nflexedge, 1, d->flexedge_J_rownnz, fp);
printArrayInt("FLEXEDGE_J_ROWADR", m->nflexedge, 1, d->flexedge_J_rowadr, fp);
printSparse("FLEXEDGE_J", d->flexedge_J, m->nflexedge, d->flexedge_J_rownnz,
d->flexedge_J_rowadr, d->flexedge_J_colind, fp, float_format);
}
printArray("FLEXEDGE_LENGTH", m->nflexedge, 1, d->flexedge_length, fp, float_format);
printArray("TEN_LENGTH", m->ntendon, 1, d->ten_length, fp, float_format);
if (!mj_isSparse(m)) {
printArray("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format);
@@ -1001,20 +1031,12 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
// contact
fprintf(fp, "CONTACT\n");
for (int i=0; i < d->ncon; i++) {
fprintf(fp, " %d:\n dim %d\n geom ", i, d->contact[i].dim);
const char* geom1 = mj_id2name(m, mjOBJ_GEOM, d->contact[i].geom1);
if (geom1) {
fprintf(fp, "%s ", geom1);
} else {
fprintf(fp, "%d ", d->contact[i].geom1);
}
const char* geom2 = mj_id2name(m, mjOBJ_GEOM, d->contact[i].geom2);
if (geom2) {
if (geom1) fprintf(fp, " "); // two spaces between two names
fprintf(fp, "%s\n", geom2);
} else {
fprintf(fp, "%d\n", d->contact[i].geom2);
}
fprintf(fp, " %d:\n dim %d\n", i, d->contact[i].dim);
fprintf(fp, " gfev %d %d %d %d : %d %d %d %d\n",
d->contact[i].geom[0], d->contact[i].flex[0],
d->contact[i].elem[0], d->contact[i].vert[0],
d->contact[i].geom[1], d->contact[i].flex[1],
d->contact[i].elem[1], d->contact[i].vert[1]);
fprintf(fp, " exclude %d\n efc_address %d\n",
d->contact[i].exclude, d->contact[i].efc_address);
printVector(" solref ", d->contact[i].solref, mjNREF, fp, float_format);
@@ -1054,6 +1076,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArray("EFC_D", d->nefc, 1, d->efc_D, fp, float_format);
printArray("EFC_R", d->nefc, 1, d->efc_R, fp, float_format);
printArray("FLEXEDGE_VELOCITY", m->nflexedge, 1, d->flexedge_velocity, fp, float_format);
printArray("TEN_VELOCITY", m->ntendon, 1, d->ten_velocity, fp, float_format);
printArray("ACTUATOR_VELOCITY", m->nu, 1, d->actuator_velocity, fp, float_format);
src/engine/engine_ray.c
+181 -3
View File
@@ -629,7 +629,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
const mjtNum* vec) {
const int meshid = m->geom_dataid[id];
const int bvhadr = m->mesh_bvhadr[meshid];
const int* faceid = m->bvh_geomid + bvhadr;
const int* faceid = m->bvh_nodeid + bvhadr;
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
const int* child = m->bvh_child + 2*bvhadr;
@@ -835,6 +835,184 @@ mjtNum mju_rayGeom(const mjtNum* pos, const mjtNum* mat, const mjtNum* size,
// intersect ray with flex, return nearest vertex id
mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert,
mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]) {
int dim = m->flex_dim[flexid];
// compute bounding box
mjtNum box[3][2] = {{0, 0}, {0, 0}, {0, 0}};
mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[flexid];
for (int i=0; i<m->flex_vertnum[flexid]; i++) {
for (int j=0; j<3; j++) {
// update minimum along side j
if (box[j][0]>vert[3*i+j] || i==0) {
box[j][0] = vert[3*i+j];
}
// update maximum along side j
if (box[j][1]<vert[3*i+j] || i==0) {
box[j][1] = vert[3*i+j];
}
}
}
// adjust box for radius
mjtNum radius = m->flex_radius[flexid];
for (int j=0; j<3; j++) {
box[j][0] -= radius;
box[j][1] += radius;
}
// construct box geom
mjtNum pos[3], size[3], mat[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
for (int j=0; j<3; j++) {
pos[j] = 0.5*(box[j][0]+box[j][1]);
size[j] = 0.5*(box[j][1]-box[j][0]);
}
// apply bounding-box filter
if (ray_box(pos, mat, size, pnt, vec, NULL)<0) {
return -1;
}
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
if (mju_abs(vec[0])>=mju_abs(vec[1]) && mju_abs(vec[0])>=mju_abs(vec[2])) {
b0[0] = 0;
} else if (mju_abs(vec[1])>=mju_abs(vec[2])) {
b0[1] = 0;
} else {
b0[2] = 0;
}
mju_addScl3(b1, b0, vec, -mju_dot3(vec, b0)/mju_dot3(vec, vec));
mju_normalize3(b1);
mju_cross(b0, b1, vec);
mju_normalize3(b0);
// init solution
mjtNum x = -1;
// check edges if rendered, or if skin
if (flg_edge || (dim>1 && flg_skin)) {
for (int e=m->flex_edgeadr[flexid];
e<m->flex_edgeadr[flexid]+m->flex_edgenum[flexid]; e++) {
// get vertices for this edge
mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid]+m->flex_edge[2*e]);
mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid]+m->flex_edge[2*e+1]);
// construct capsule geom
mju_add3(pos, v1, v2);
mju_scl3(pos, pos, 0.5);
mjtNum dif[3] = {v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]};
size[0] = radius;
size[1] = 0.5*mju_normalize3(dif);
mjtNum quat[4];
mju_quatZ2Vec(quat, dif);
mju_quat2Mat(mat, quat);
// intersect ray with capsule
mjtNum sol = mju_rayGeom(pos, mat, size, pnt, vec, mjGEOM_CAPSULE);
// update
if (sol>=0 && (x<0 || sol<x)) {
x = sol;
// construct intersection point
mjtNum intersect[3];
mju_addScl3(intersect, pnt, vec, sol);
// find nearest vertex
if (mju_dist3(v1, intersect) < mju_dist3(v2, intersect)) {
*vertid = m->flex_edge[2*e];
}
else {
*vertid = m->flex_edge[2*e+1];
}
}
}
}
// check vertices if rendered (and edges not checked)
else if (flg_vert && !(dim>1 && flg_skin)) {
for (int v=0; v<m->flex_vertnum[flexid]; v++) {
// get vertex
mjtNum* vpos = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + v);
// construct sphere geom
size[0] = radius;
// intersect ray with sphere
mjtNum sol = mju_rayGeom(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE);
// update
if (sol>=0 && (x<0 || sol<x)) {
x = sol;
*vertid = v;
}
}
}
// check faces if rendered
if (dim>1 && (flg_face || flg_skin)) {
for (int e=0; e<m->flex_elemnum[flexid]; e++) {
// skip if 3D element is not visible
int elayer = m->flex_elemlayer[m->flex_elemadr[flexid]+e];
if (dim==3 && ((flg_skin && elayer>0) || (!flg_skin && elayer!=flex_layer))) {
continue;
}
// get element data
const int* edata = m->flex_elem + m->flex_elemdataadr[flexid] + e*(dim+1);
mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[0]);
mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[1]);
mjtNum* v3 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[2]);
mjtNum* v4 = dim==2 ? NULL : d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[3]);
mjtNum* vptr[4][3] = {{v1, v2, v3}, {v1, v2, v4}, {v1, v3, v4}, {v2, v3, v4}};
int vid[4][3] = {{0, 1, 2}, {0, 1, 3}, {0, 2, 3}, {1, 2, 3}};
// process triangles of this element
for (int i=0; i<(dim==2?1:4); i++) {
// copy vertices into triangle representation
mjtNum v[3][3];
for (int j=0; j<3; j++)
mju_copy3(v[j], vptr[i][j]);
// intersect ray with triangle
mjtNum sol = ray_triangle(v, pnt, vec, b0, b1);
// update
if (sol>=0 && (x<0 || sol<x)) {
x = sol;
// construct intersection point
mjtNum intersect[3];
mju_addScl3(intersect, pnt, vec, sol);
// find nearest vertex
mjtNum dist[3] = {
mju_dist3(v[0], intersect),
mju_dist3(v[1], intersect),
mju_dist3(v[2], intersect)
};
if (dist[0]<=dist[1] && dist[0]<=dist[2]) {
*vertid = edata[vid[i][0]];
} else if (dist[1]<=dist[2]){
*vertid = edata[vid[i][1]];
} else {
*vertid = edata[vid[i][2]];
}
}
}
}
}
return x;
}
// intersect ray with skin, return nearest vertex id
mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]) {
@@ -881,7 +1059,7 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
mju_normalize3(b0);
// init solution
mjtNum x = -1, sol;
mjtNum x = -1;
// process all faces
for (int i=0; i < nface; i++) {
@@ -900,7 +1078,7 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
}
// solve
sol = ray_triangle(v, pnt, vec, b0, b1);
mjtNum sol = ray_triangle(v, pnt, vec, b0, b1);
// update
if (sol >= 0 && (x < 0 || sol < x)) {
src/engine/engine_ray.h
+5
View File
@@ -58,6 +58,11 @@ MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
MJAPI mjtNum mju_rayGeom(const mjtNum* pos, const mjtNum* mat, const mjtNum* size,
const mjtNum* pnt, const mjtNum* vec, int geomtype);
// intersect ray with flex, return nearest vertex id
MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert,
mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]);
// intersect ray with skin, return nearest vertex id
MJAPI mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]);
src/engine/engine_sensor.c
+24 -6
View File
@@ -662,7 +662,7 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
// acceleration/force-dependent sensors
void mj_sensorAcc(const mjModel* m, mjData* d) {
int rootid, bodyid, objtype, objid, body1, body2, adr, nusersensor = 0;
int rootid, bodyid, objtype, objid, adr, nusersensor = 0;
int ne = d->ne, nf = d->nf, nefc = d->nefc;
mjtNum tmp[6], conforce[6], conray[3];
mjContact* con;
@@ -713,13 +713,15 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// find contacts in sensor zone, add normal forces
for (int j=0; j < d->ncon; j++) {
// contact pointer, contacting bodies
// contact pointer, contacting bodies (-1 for flex)
con = d->contact + j;
body1 = m->geom_bodyid[con->geom1];
body2 = m->geom_bodyid[con->geom2];
int conbody[2];
for (int k=0; k<2; k++) {
conbody[k] = (con->geom[k]>=0) ? m->geom_bodyid[con->geom[k]] : -1;
}
// select contacts involving sensorized body
if (con->efc_address >= 0 && (bodyid == body1 || bodyid == body2)) {
if (con->efc_address >= 0 && (bodyid == conbody[0] || bodyid == conbody[1])) {
// get contact force:torque in contact frame
mj_contactForce(m, d, j, conforce);
@@ -733,7 +735,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
mju_normalize3(conray);
// flip ray direction if sensor is on body2
if (bodyid == body2) {
if (bodyid == conbody[1]) {
mju_scl3(conray, conray, -1);
}
@@ -945,6 +947,22 @@ void mj_energyPos(const mjModel* m, mjData* d) {
d->energy[0] += 0.5*stiffness*displacement*displacement;
}
}
// add flex-level springs
if (!mjDISABLED(mjDSBL_PASSIVE)) {
for (int i=0; i<m->nflex; i++) {
stiffness = m->flex_edgestiffness[i];
if (m->flex_rigid[i] || stiffness==0) {
continue;
}
// process edges of this flex
for (int e=m->flex_edgeadr[i]; e<m->flex_edgeadr[i]+m->flex_edgenum[i]; e++) {
mjtNum displacement = m->flexedge_length0[e] - d->flexedge_length[e];
d->energy[0] += 0.5*stiffness*displacement*displacement;
}
}
}
}
src/engine/engine_setconst.c
+50 -3
View File
@@ -95,6 +95,7 @@ static void set0(mjModel* m, mjData* d) {
// run remaining computations
mj_crb(m, d);
mj_factorM(m, d);
mj_flex(m, d);
mj_tendon(m, d);
mj_transmission(m, d);
@@ -106,7 +107,9 @@ static void set0(mjModel* m, mjData* d) {
m->light_mode[i] = lightmode[i];
}
// set tendon_length0, actuator_length0
// copy fields
mju_copy(m->flex_xvert0, d->flexvert_xpos, 3*m->nflexvert);
mju_copy(m->flexedge_length0, d->flexedge_length, m->nflexedge);
mju_copy(m->tendon_length0, d->ten_length, m->ntendon);
mju_copy(m->actuator_length0, d->actuator_length, m->nu);
@@ -194,9 +197,42 @@ static void set0(mjModel* m, mjData* d) {
}
}
// compute tendon_invweight0
// compute flexedge_invweight0, tendon_invweight0, actuator_acc0
if (nv) {
for (int i=0; i < m->ntendon; i++) {
// compute flexedge_invweight0
for (int f=0; f<m->nflex; f++) {
for (int i=m->flex_edgeadr[f]; i<m->flex_edgeadr[f]+m->flex_edgenum[f]; i++) {
// bodies connected by edge
int b1 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i+1]];
// accelerate edges that connect simple bodies with no rotations
if (m->body_simple[b1]==2 && m->body_simple[b2]==2) {
m->flexedge_invweight0[i] = (1/m->body_mass[b1] + 1/m->body_mass[b2])/2;
}
// handle general edge
else {
// make dense vector into tmp
if (mj_isSparse(m)) {
mju_zero(tmp, nv);
int end = d->flexedge_J_rowadr[i] + d->flexedge_J_rownnz[i];
for (int j=d->flexedge_J_rowadr[i]; j<end; j++) {
tmp[d->flexedge_J_colind[j]] = d->flexedge_J[j];
}
} else {
mju_copy(tmp, d->flexedge_J+i*nv, nv);
}
// solve into tmp+nv
mj_solveM(m, d, tmp+nv, tmp, 1);
m->flexedge_invweight0[i] = mju_dot(tmp, tmp+nv, nv);
}
}
}
// compute tendon_invweight0
for (int i=0; i<m->ntendon; i++) {
// make dense vector into tmp
if (mj_isSparse(m)) {
mju_zero(tmp, nv);
@@ -385,6 +421,17 @@ static void setStat(mjModel* m, mjData* d) {
}
}
// adjust body size for flex edges involving body
for (int f=0; f<m->nflex; f++) {
for (int e=m->flex_edgeadr[f]; e<m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
int b1 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e+1]];
body[b1] = mju_max(body[b1], m->flexedge_length0[e]);
body[b2] = mju_max(body[b2], m->flexedge_length0[e]);
}
}
// compute meansize, make sure all sizes are above min
if (m->nbody > 1) {
m->stat.meansize = 0;
src/engine/engine_support.c
+244 -12
View File
@@ -233,6 +233,131 @@ void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int
//-------------------------- sparse chains ---------------------------------------------------------
// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) {
int da1, da2, NV = 0;
// skip fixed bodies
while (b1 && !m->body_dofnum[b1]) {
b1 = m->body_parentid[b1];
}
while (b2 && !m->body_dofnum[b2]) {
b2 = m->body_parentid[b2];
}
// neither body is movable: empty chain
if (b1==0 && b2==0) {
return 0;
}
// intialize last dof address for each body
da1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1;
da2 = m->body_dofadr[b2] + m->body_dofnum[b2] - 1;
// merge chains
while (da1>=0 || da2>=0) {
chain[NV] = mjMAX(da1, da2);
if (da1==chain[NV]) {
da1 = m->dof_parentid[da1];
}
if (da2==chain[NV]) {
da2 = m->dof_parentid[da2];
}
NV++;
}
// reverse order of chain: make it increasing
for (int i=0; i<NV/2; i++) {
int tmp = chain[i];
chain[i] = chain[NV-i-1];
chain[NV-i-1] = tmp;
}
return NV;
}
// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) {
// swap bodies if wrong order
if (b1>b2) {
int tmp = b1;
b1 = b2;
b2 = tmp;
}
// init
int n1 = m->body_dofnum[b1], n2 = m->body_dofnum[b2];
// both fixed: nothing to do
if (n1==0 && n2==0) {
return 0;
}
// copy b1 dofs
for (int i=0; i<n1; i++) {
chain[i] = m->body_dofadr[b1] + i;
}
// copy b2 dofs
for (int i=0; i<n2; i++) {
chain[n1+i] = m->body_dofadr[b2] + i;
}
return (n1+n2);
}
// get body chain
int mj_bodyChain(const mjModel* m, int body, int* chain) {
// simple body
if (m->body_simple[body]) {
int dofnum = m->body_dofnum[body];
for (int i=0; i<dofnum; i++) {
chain[i] = m->body_dofadr[body] + i;
}
return dofnum;
}
// general case
else {
// skip fixed bodies
while (body && !m->body_dofnum[body]) {
body = m->body_parentid[body];
}
// not movable: empty chain
if (body==0) {
return 0;
}
// intialize last dof
int da = m->body_dofadr[body] + m->body_dofnum[body] - 1;
int NV = 0;
// construct chain from child to parent
while (da>=0) {
chain[NV++] = da;
da = m->dof_parentid[da];
}
// reverse order of chain: make it increasing
for (int i=0; i<NV/2; i++) {
int tmp = chain[i];
chain[i] = chain[NV-i-1];
chain[NV-i-1] = tmp;
}
return NV;
}
}
//-------------------------- Jacobians -------------------------------------------------------------
// compute 3/6-by-nv Jacobian of global point attached to given body
@@ -374,7 +499,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA
// compute 3/6-by-nv sparse Jacobian of global point attached to given body
void mj_jacSparse(const mjModel* m, const mjData* d,
mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body,
int NV, int* chain) {
int NV, const int* chain) {
int da, ci;
mjtNum offset[3], tmp[3], *cdof = d->cdof;
@@ -578,6 +703,80 @@ int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
// dense or sparse weighted sum of multiple body Jacobians at same point
int mj_jacSum(const mjModel* m, mjData* d, int* chain,
int n, const int* body, const mjtNum* weight,
const mjtNum point[3], mjtNum* jac, int flg_rot) {
int nv = m->nv, NV;
mjtNum* jacp = jac;
mjtNum* jacr = flg_rot ? jac + 3*nv : NULL;
mj_markStack(d);
mjtNum* jtmp = mj_stackAllocNum(d, flg_rot ? 6*nv : 3*nv);
mjtNum* jp = jtmp;
mjtNum* jr = flg_rot ? jtmp + 3*nv : NULL;
// sparse
if (mj_isSparse(m)) {
mjtNum* buf = mj_stackAllocNum(d, flg_rot ? 6*nv : 3*nv);
int* buf_ind = mj_stackAllocInt(d, nv);
int* bodychain = mj_stackAllocInt(d, nv);
// set first
NV = mj_bodyChain(m, body[0], chain);
if (NV) {
// get Jacobian
if (m->body_simple[body[0]]) {
mj_jacSparseSimple(m, d, jacp, jacr, point, body[0], 1, NV, 0);
} else {
mj_jacSparse(m, d, jacp, jacr, point, body[0], NV, chain);
}
// apply weight
mju_scl(jac, jac, weight[0], flg_rot ? 6*NV : 3*NV);
}
// accumulate remaining
for (int i=1; i<n; i++) {
// get body chain and Jacobian
int bodyNV = mj_bodyChain(m, body[i], bodychain);
if (!bodyNV) {
continue;
}
if (m->body_simple[body[i]]) {
mj_jacSparseSimple(m, d, jp, jr, point, body[i], 1, bodyNV, 0);
} else {
mj_jacSparse(m, d, jp, jr, point, body[i], bodyNV, bodychain);
}
// combine sparse matrices
NV = mju_addToSparseMat(jac, jtmp, nv, flg_rot ? 6 : 3, weight[i],
NV, bodyNV, chain, bodychain, buf, buf_ind);
}
}
// dense
else {
// set first
mj_jac(m, d, jacp, jacr, point, body[0]);
mju_scl(jac, jac, weight[0], flg_rot ? 6*nv : 3*nv);
// accumulate remaining
for (int i=1; i<n; i++) {
mj_jac(m, d, jp, jr, point, body[i]);
mju_addToScl(jac, jtmp, weight[i], flg_rot ? 6*nv : 3*nv);
}
NV = nv;
}
mj_freeStack(d);
return NV;
}
//-------------------------- name functions --------------------------------------------------------
// get number of objects and name addresses for given object type
@@ -635,6 +834,14 @@ static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) {
}
mjFALLTHROUGH;
case mjOBJ_FLEX:
*mapadr -= mjLOAD_MULTIPLE*m->nflex;
if (num < 0) {
*padr = m->name_flexadr;
num = m->nflex;
}
mjFALLTHROUGH;
case mjOBJ_MESH:
*mapadr -= mjLOAD_MULTIPLE*m->nmesh;
if (num < 0) {
@@ -1251,8 +1458,8 @@ void mj_applyFT(const mjModel* m, mjData* d,
// allocate local variables
mj_markStack(d);
mjtNum* jacp = mj_stackAllocNum(d, 3*nv);
mjtNum* jacr = mj_stackAllocNum(d, 3*nv);
mjtNum* jacp = force ? mj_stackAllocNum(d, 3*nv) : NULL;
mjtNum* jacr = torque ? mj_stackAllocNum(d, 3*nv) : NULL;
mjtNum* qforce = mj_stackAllocNum(d, nv);
// make sure body is in range
@@ -1260,17 +1467,42 @@ void mj_applyFT(const mjModel* m, mjData* d,
mjERROR("invalid body %d", body);
}
// compute Jacobians
mj_jac(m, d, jacp, jacr, point, body);
// sparse case
if (mj_isSparse(m)) {
// construct chain and sparse Jacobians
int* chain = mj_stackAllocInt(d, nv);
int NV = mj_bodyChain(m, body, chain);
mj_jacSparse(m, d, jacp, jacr, point, body, NV, chain);
// compute J'*f and accumulate
if (force) {
mju_mulMatTVec(qforce, jacp, force, 3, nv);
mju_addTo(qfrc_target, qforce, nv);
// compute J'*f and accumulate
if (force) {
mju_mulMatTVec(qforce, jacp, force, 3, NV);
for (int i=0; i<NV; i++) {
qfrc_target[chain[i]] += qforce[i];
}
}
if (torque) {
mju_mulMatTVec(qforce, jacr, torque, 3, NV);
for (int i=0; i<NV; i++) {
qfrc_target[chain[i]] += qforce[i];
}
}
}
if (torque) {
mju_mulMatTVec(qforce, jacr, torque, 3, nv);
mju_addTo(qfrc_target, qforce, nv);
// dense case
else {
// compute Jacobians
mj_jac(m, d, jacp, jacr, point, body);
// compute J'*f and accumulate
if (force) {
mju_mulMatTVec(qforce, jacp, force, 3, nv);
mju_addTo(qfrc_target, qforce, nv);
}
if (torque) {
mju_mulMatTVec(qforce, jacr, torque, 3, nv);
mju_addTo(qfrc_target, qforce, nv);
}
}
mj_freeStack(d);
src/engine/engine_support.h
+18 -1
View File
@@ -44,6 +44,18 @@ MJAPI void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigne
MJAPI void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int spec);
//-------------------------- sparse chains ---------------------------------------------------------
// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2);
// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2);
// get body chain
int mj_bodyChain(const mjModel* m, int body, int* chain);
//-------------------------- Jacobians -------------------------------------------------------------
// compute 3/6-by-nv Jacobian of global point attached to given body
@@ -77,7 +89,7 @@ MJAPI void mj_jacPointAxis(const mjModel* m, mjData* d,
// compute 3/6-by-nv sparse Jacobian of global point attached to given body
void mj_jacSparse(const mjModel* m, const mjData* d,
mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body,
int NV, int* chain);
int NV, const int* chain);
// sparse Jacobian difference for simple body contacts
void mj_jacSparseSimple(const mjModel* m, const mjData* d,
@@ -90,6 +102,11 @@ MJAPI int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
mjtNum* jac1p, mjtNum* jac2p, mjtNum* jacdifp,
mjtNum* jac1r, mjtNum* jac2r, mjtNum* jacdifr);
// dense or sparse weighted sum of multiple body Jacobians at same point
int mj_jacSum(const mjModel* m, mjData* d, int* chain,
int n, const int* body, const mjtNum* weight,
const mjtNum point[3], mjtNum* jac, int flg_rot);
//-------------------------- name functions --------------------------------------------------------
src/engine/engine_util_container.c
+3 -3
View File
@@ -68,7 +68,8 @@ void mju_arrayListAdd(mjArrayList* array_list, void* element) {
cursor = cursor->next_segment;
}
// copy element into segment
memcpy(cursor->buffer + cursor->element_size * cursor->size, element, cursor->element_size);
memcpy((mjtByte*)cursor->buffer + cursor->element_size * cursor->size,
element, cursor->element_size);
++cursor->size;
}
@@ -92,7 +93,6 @@ void* mju_arrayListAt(const mjArrayList* array_list, size_t index) {
return NULL;
}
return cursor->buffer +
(cursor->element_size * (index - total_capacity));
return (mjtByte*)cursor->buffer + (cursor->element_size * (index - total_capacity));
}
src/engine/engine_util_errmem.h
+1
View File
@@ -72,6 +72,7 @@ MJAPI void mju_writeLog(const char* type, const char* msg);
//------------------------------ internal error macros --------------------------------------------
// internal macro to prepend the calling function name to the error message
#pragma warning(disable : 4996) // needed to use strncpy with Visual Studio
#define mjERROR(...) \
{ \
char _errbuf[1024]; \
src/engine/engine_util_misc.c
+7
View File
@@ -954,6 +954,9 @@ const char* mju_type2Str(int type) {
case mjOBJ_LIGHT:
return "light";
case mjOBJ_FLEX:
return "flex";
case mjOBJ_MESH:
return "mesh";
@@ -1043,6 +1046,10 @@ int mju_str2Type(const char* str) {
return mjOBJ_LIGHT;
}
else if (!strcmp(str, "flex")) {
return mjOBJ_FLEX;
}
else if (!strcmp(str, "mesh")) {
return mjOBJ_MESH;
}
src/engine/engine_util_sparse.c
+170
View File
@@ -368,6 +368,176 @@ void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNu
// dst += src, only at common non-zero indices
void mju_addToSparseInc(mjtNum* dst, const mjtNum* src,
int nnzdst, const int* inddst,
int nnzsrc, const int* indsrc) {
if (!nnzdst || !nnzsrc) {
return;
}
int adrs = 0, adrd = 0, inds = indsrc[0], indd = inddst[0];
while (1) {
// common non-zero index
if (inds==indd) {
// add
dst[adrd] += src[adrs];
// advance src
if (++adrs<nnzsrc) {
inds = indsrc[adrs];
} else {
return;
}
// advance dst
if (++adrd<nnzdst) {
indd = inddst[adrd];
} else {
return;
}
}
// src non-zero index smaller: advance src
else if (inds<indd) {
if (++adrs<nnzsrc) {
inds = indsrc[adrs];
} else {
return;
}
}
// dst non-zero index smaller: advance dst
else {
if (++adrd<nnzdst) {
indd = inddst[adrd];
} else {
return;
}
}
}
}
// add to sparse matrix: dst = dst + scl*src, return nnz of result
int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum scl,
int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind,
mjtNum* buf, int* buf_ind) {
// check for identical pattern
if (dst_nnz==src_nnz) {
if (dst_nnz==0) {
return 0;
}
if (mju_compare(dst_ind, src_ind, dst_nnz)) {
// combine mjtNum data directly
mju_addToScl(dst, src, scl, nrow*dst_nnz);
return dst_nnz;
}
}
// prepare to merge scr and dst into buf^T
int si = 0, di = 0, nnz = 0;
int sadr = src_nnz ? src_ind[0] : n+1;
int dadr = dst_nnz ? dst_ind[0] : n+1;
// merge matrices
while (si<src_nnz || di<dst_nnz) {
// both
if (sadr==dadr) {
for (int k=0; k<nrow; k++) {
buf[nrow*nnz + k] = dst[di + k*dst_nnz] + scl*src[si + k*src_nnz];
}
buf_ind[nnz++] = sadr;
si++;
di++;
sadr = si<src_nnz ? src_ind[si] : n+1;
dadr = di<dst_nnz ? dst_ind[di] : n+1;
}
// dst only
else if (dadr<sadr) {
for (int k=0; k<nrow; k++) {
buf[nrow*nnz + k] = dst[di + k*dst_nnz];
}
buf_ind[nnz++] = dadr;
di++;
dadr = di<dst_nnz ? dst_ind[di] : n+1;
}
// src only
else {
for (int k=0; k<nrow; k++) {
buf[nrow*nnz + k] = scl*src[si + k*src_nnz];
}
buf_ind[nnz++] = sadr;
si++;
sadr = si<src_nnz ? src_ind[si] : n+1;
}
}
// copy transposed buf into dst
mju_transpose(dst, buf, nnz, nrow);
mju_copyInt(dst_ind, buf_ind, nnz);
return nnz;
}
// add(merge) two chains
int mju_addChains(int* res, int n, int NV1, int NV2,
const int* chain1, const int* chain2) {
// check for identical pattern
if (NV1==NV2) {
if (NV1==0) {
return 0;
}
if (mju_compare(chain1, chain2, NV1)) {
mju_copyInt(res, chain1, NV1);
return NV1;
}
}
// prepare to merge
int i1 = 0, i2 = 0, NV = 0;
int adr1 = NV1 ? chain1[0] : n+1;
int adr2 = NV2 ? chain2[0] : n+1;
// merge chains
while (i1<NV1 || i2<NV2) {
// both
if (adr1==adr2) {
res[NV++] = adr1;
i1++;
i2++;
adr1 = i1<NV1 ? chain1[i1] : n+1;
adr2 = i2<NV2 ? chain2[i2] : n+1;
}
// chain1 only
else if (adr1<adr2) {
res[NV++] = adr1;
i1++;
adr1 = i1<NV1 ? chain1[i1] : n+1;
}
// chain2 only
else {
res[NV++] = adr2;
i2++;
adr2 = i2<NV2 ? chain2[i2] : n+1;
}
}
return NV;
}
// compress layout of sparse matrix
void mju_compressSparse(mjtNum* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind) {
rowadr[0] = 0;
src/engine/engine_util_sparse.h
+14
View File
@@ -63,6 +63,20 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b,
void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b,
int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind);
// dst += src, only at common non-zero indices
void mju_addToSparseInc(mjtNum* dst, const mjtNum* src,
int nnzdst, const int* inddst,
int nnzsrc, const int* indsrc);
// add to sparse matrix: dst = dst + scl*src, return nnz of result
int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum scl,
int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind,
mjtNum* buf, int* buf_ind);
// add(merge) two chains
int mju_addChains(int* res, int n, int NV1, int NV2,
const int* chain1, const int* chain2);
// transpose sparse matrix
MJAPI void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
int* res_rownnz, int* res_rowadr, int* res_colind,
src/engine/engine_vis_init.c
+120 -14
View File
@@ -43,6 +43,7 @@ const char* mjLABELSTRING[mjNLABEL] = {
"Tendon",
"Actuator",
"Constraint",
"Flex",
"Skin",
"Selection",
"SelPoint",
@@ -91,8 +92,13 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
{"S&elect Point", "0", "E"},
{"Static Bo&dy", "1", "D"},
{"Skin", "1", ";"},
{"Flex Vert", "1", "."},
{"Flex Edge", "1", ""},
{"Flex Face", "1", ""},
{"Flex Skin", "0", ""},
{"Body Tree", "0", "`"},
{"Mesh Tree", "0", ""},
{"Flex Tree", "0", ""},
{"Mesh Tree", "0", "\\"},
{"SDF iters", "0", ""}
};
@@ -107,7 +113,7 @@ const char* mjRNDSTRING[mjNRNDFLAG][3] = {
{"Fog", "0", "G"},
{"Haze", "1", "/"},
{"Segment", "0", ","},
{"Id Color", "0", "."},
{"Id Color", "0", ""},
{"Cull Face", "1", ""}
};
@@ -143,6 +149,96 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
scn->scale = 1;
scn->rotate[0] = 1;
// set number of flexes
scn->nflex = m ? m->nflex : 0;
// allocate flex data
if (scn->nflex) {
int nflex = scn->nflex;
// allocate fixed
scn->flexedgeadr = (int*) mju_malloc(nflex*sizeof(int));
scn->flexedgenum = (int*) mju_malloc(nflex*sizeof(int));
scn->flexvertadr = (int*) mju_malloc(nflex*sizeof(int));
scn->flexvertnum = (int*) mju_malloc(nflex*sizeof(int));
scn->flexfaceadr = (int*) mju_malloc(nflex*sizeof(int));
scn->flexfacenum = (int*) mju_malloc(nflex*sizeof(int));
scn->flexfaceused= (int*) mju_malloc(nflex*sizeof(int));
scn->flexedge = (int*) mju_malloc(2*m->nflexedge*sizeof(int));
scn->flexvert = (float*) mju_malloc(3*m->nflexvert*sizeof(float));
// count max number of flex faces to be rendered (depending on vis options)
int nface = 0;
for (int f=0; f<nflex; f++) {
// 1D : 0
if (m->flex_dim[f]==0) {
scn->flexfacenum[f] = 0;
}
// 2D: 2*fragments + 2*elements
else if (m->flex_dim[f]==2) {
scn->flexfacenum[f] = 2*m->flex_shellnum[f] + 2*m->flex_elemnum[f];
}
// 3D: max(fragments, 4*maxlayer)
else {
// find number of elements in biggest layer
int maxlayer = 0, layer = 0, nlayer = 1;
while (nlayer) {
// count elements in this layer
nlayer = 0;
for (int e=0; e<m->flex_elemnum[f]; e++) {
if (m->flex_elemlayer[m->flex_elemadr[f]+e]==layer) {
nlayer++;
}
}
// accumulate max over layers, advance layer
maxlayer = mjMAX(maxlayer, nlayer);
layer++;
}
scn->flexfacenum[f] = mjMAX(m->flex_shellnum[f], 4*maxlayer);
}
// accumulate over flexes
nface += scn->flexfacenum[f];
}
// allocate face-related
scn->flexface = nface ? (float*) mju_malloc(9*nface*sizeof(float)) : NULL;
scn->flexnormal = nface ? (float*) mju_malloc(9*nface*sizeof(float)) : NULL;
scn->flextexcoord = nface ? (float*) mju_malloc(6*nface*sizeof(float)) : NULL;
// check allocation
if (!scn->flexedgeadr ||
!scn->flexedgenum ||
!scn->flexfaceadr ||
!scn->flexfacenum ||
!scn->flexfaceused||
!scn->flexvertadr ||
!scn->flexvertnum ||
!scn->flexedge ||
!scn->flexvert ||
(nface && !scn->flexface) ||
(nface && !scn->flexnormal) ||
(nface && !scn->flextexcoord)) {
mju_error("Could not allocate flex buffers");
}
// copy constant edge and vertex data
memcpy(scn->flexedgeadr, m->flex_edgeadr, nflex*sizeof(int));
memcpy(scn->flexedgenum, m->flex_edgenum, nflex*sizeof(int));
memcpy(scn->flexvertadr, m->flex_vertadr, nflex*sizeof(int));
memcpy(scn->flexvertnum, m->flex_vertnum, nflex*sizeof(int));
memcpy(scn->flexedge, m->flex_edge, 2*m->nflexedge*sizeof(int));
// compute flexfaceadr
for (int f=0; f<nflex; f++) {
scn->flexfaceadr[f] = f==0 ? 0 : scn->flexfaceadr[f-1]+scn->flexfacenum[f-1];
}
}
// set number of skins
if (m) {
scn->nskin = m->nskin;
@@ -152,19 +248,14 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
// allocate skin data
if (scn->nskin) {
// compute number of vertices in all skins
int nskin = m->nskin;
int totvert = 0;
for (int i=0; i < nskin; i++) {
totvert += m->skin_vertnum[i];
}
// allocate
scn->skinfacenum = (int*) mju_malloc(nskin*sizeof(int));
scn->skinvertadr = (int*) mju_malloc(nskin*sizeof(int));
scn->skinvertnum = (int*) mju_malloc(nskin*sizeof(int));
scn->skinvert = (float*) mju_malloc(3*totvert*sizeof(float));
scn->skinnormal = (float*) mju_malloc(3*totvert*sizeof(float));
scn->skinvert = (float*) mju_malloc(3*m->nskinvert*sizeof(float));
scn->skinnormal = (float*) mju_malloc(3*m->nskinvert*sizeof(float));
// check allocation
if (!scn->skinfacenum ||
@@ -176,11 +267,9 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
}
// copy constant data
for (int i=0; i < nskin; i++) {
scn->skinfacenum[i] = m->skin_facenum[i];
scn->skinvertadr[i] = m->skin_vertadr[i];
scn->skinvertnum[i] = m->skin_vertnum[i];
}
mju_copyInt(scn->skinfacenum, m->skin_facenum, nskin);
mju_copyInt(scn->skinvertadr, m->skin_vertadr, nskin);
mju_copyInt(scn->skinvertnum, m->skin_vertnum, nskin);
}
// mjvGeom, mjvLight, mjvGLCamera objects are invalid
@@ -193,6 +282,20 @@ void mjv_freeScene(mjvScene* scn) {
// free buffers allocated by mjv_makeScene
mju_free(scn->geoms);
mju_free(scn->geomorder);
mju_free(scn->flexedgeadr);
mju_free(scn->flexedgenum);
mju_free(scn->flexvertadr);
mju_free(scn->flexvertnum);
mju_free(scn->flexfaceadr);
mju_free(scn->flexfacenum);
mju_free(scn->flexfaceused);
mju_free(scn->flexedge);
mju_free(scn->flexvert);
mju_free(scn->flexface);
mju_free(scn->flexnormal);
mju_free(scn->flextexcoord);
mju_free(scn->skinfacenum);
mju_free(scn->skinvertadr);
mju_free(scn->skinvertnum);
@@ -224,6 +327,7 @@ void mjv_defaultOption(mjvOption* vopt) {
vopt->jointgroup[i] = state;
vopt->tendongroup[i] = state;
vopt->actuatorgroup[i] = state;
vopt->flexgroup[i] = state;
vopt->skingroup[i] = state;
}
@@ -232,6 +336,7 @@ void mjv_defaultOption(mjvOption* vopt) {
}
vopt->bvh_depth = 1;
vopt->flex_layer = 0;
}
@@ -271,6 +376,7 @@ void mjv_defaultFreeCamera(const mjModel* m, mjvCamera* cam) {
void mjv_defaultPerturb(mjvPerturb* pert) {
memset(pert, 0, sizeof(mjvPerturb));
pert->flexselect = -1;
pert->skinselect = -1;
pert->refquat[0] = 1;
pert->scale = 1;
src/engine/engine_vis_interact.c
+66 -11
View File
@@ -545,6 +545,12 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
mju_dot(jacM2+2*nv, jacM2+2*nv, nv);
pert->localmass = 3 / mju_max(invmass, mjMINVAL);
// scale localmass with flex average number of edges per vertex
if (pert->flexselect>=0 && !m->flex_rigid[pert->flexselect]) {
pert->localmass *= (2.0*m->flex_edgenum[pert->flexselect]) /
(mjtNum)m->flex_vertnum[pert->flexselect];
}
// copy
mju_copy3(pert->refpos, d->xipos+3*sel);
mju_mulQuat(pert->refquat, d->xquat+4*sel, m->body_iquat+4*sel);
@@ -731,10 +737,11 @@ mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2)
// Select geom or skin with mouse, return bodyid; -1: none selected.
// Select geom, flex or skin with mouse, return bodyid; -1: none selected.
int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]) {
const mjvScene* scn, mjtNum selpnt[3],
int geomid[1], int flexid[1], int skinid[1]) {
// get average camera
mjvGLCamera cam = mjv_averageCamera(scn->camera, scn->camera+1);
@@ -759,9 +766,35 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum geomdist = mj_ray(m, d, pos, ray, vopt->geomgroup,
vopt->flags[mjVIS_STATIC], -1, geomid);
// find intersection with flexes
int flexbodyid = -1;
mjtNum flexdist = -1;
mjtNum flexpnt[3] = {0, 0, 0};
*flexid = -1;
if (vopt->flags[mjVIS_FLEXVERT] || vopt->flags[mjVIS_FLEXEDGE] ||
vopt->flags[mjVIS_FLEXFACE] || vopt->flags[mjVIS_FLEXSKIN]) {
for (int i=0; i<m->nflex; i++) {
// process one flex
int vertid;
mjtNum newdist = mju_rayFlex(m, d, vopt->flex_layer,
vopt->flags[mjVIS_FLEXVERT], vopt->flags[mjVIS_FLEXEDGE],
vopt->flags[mjVIS_FLEXFACE], vopt->flags[mjVIS_FLEXSKIN],
i, pos, ray, &vertid);
// update if closer intersection found
if (newdist>=0 && (newdist<flexdist || flexdist<0)) {
flexdist = newdist;
flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
*flexid = i;
mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[i] + vertid));
}
}
}
// find intersection with skins
int bodyid = -1;
int skinbodyid = -1;
mjtNum skindist = -1;
mjtNum skinpnt[3] = {0, 0, 0};
*skinid = -1;
if (vopt->flags[mjVIS_SKIN]) {
for (int i=0; i < m->nskin; i++) {
@@ -774,7 +807,6 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// update if closer intersection found
if (newdist >= 0 && (newdist < skindist || skindist < 0)) {
// assign result
skindist = newdist;
// find body with largest weight for this vertex
@@ -792,8 +824,9 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// update if matching id and bigger weight
if (vid == vertid && vweight > bestweight) {
bestweight = vweight;
bodyid = m->skin_bonebodyid[j];
skinbodyid = m->skin_bonebodyid[j];
*skinid = i;
mju_f2n(skinpnt, scn->skinvert + 3*(m->skin_vertadr[i] + vertid), 3);
}
}
}
@@ -802,21 +835,43 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
}
// no intersection
if (geomdist < 0 && skindist < 0) {
if (geomdist < 0 && flexdist < 0 && skindist < 0) {
return -1;
}
// geom only, or geom closer than skin
else if (geomdist >= 0 && (skindist < 0 || skindist > geomdist)) {
mju_addScl3(selpnt, pos, ray, geomdist);
// find smallest non-negative distance
mjtNum raydist[3] = {geomdist, flexdist, skindist};
int best = -1;
for (int i=0; i<3; i++) {
if (raydist[i]>=0) {
if (best<0 || raydist[best]>raydist[i]) {
best = i;
}
}
}
// geom
if (best == 0) {
*flexid = -1;
*skinid = -1;
mju_addScl3(selpnt, pos, ray, raydist[best]);
return m->geom_bodyid[*geomid];
}
// flex
else if (best == 1) {
*geomid = -1;
*skinid = -1;
mju_copy3(selpnt, flexpnt);
return flexbodyid;
}
// skin
else {
mju_addScl3(selpnt, pos, ray, skindist);
*geomid = -1;
return bodyid;
*flexid = -1;
mju_copy3(selpnt, skinpnt);
return skinbodyid;
}
}
src/engine/engine_vis_interact.h
+3 -2
View File
@@ -72,10 +72,11 @@ MJAPI void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb*
// return the average of two OpenGL cameras
MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
// Select geom or skin with mouse, return bodyid; -1: none selected.
// Select geom, flex or skin with mouse, return bodyid; -1: none selected.
MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
const mjvScene* scn, mjtNum selpnt[3],
int geomid[1], int flexid[1], int skinid[1]);
#ifdef __cplusplus
}
src/engine/engine_vis_state.c
+6
View File
@@ -247,6 +247,12 @@ int mjv_updateSceneFromState(const mjvSceneState* scnstate, const mjvOption* opt
// update camera
mjv_updateCamera(&m, &d, cam, scn);
// update flexes
if (opt->flags[mjVIS_FLEXVERT] || opt->flags[mjVIS_FLEXEDGE] ||
opt->flags[mjVIS_FLEXFACE] || opt->flags[mjVIS_FLEXSKIN]) {
mjv_updateActiveFlex(&m, &d, scn, opt);
}
// update skins
if (opt->flags[mjVIS_SKIN]) {
mjv_updateActiveSkin(&m, &d, scn, opt);
src/engine/engine_vis_visualize.c
+494 -57
View File
@@ -22,6 +22,7 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjvisualize.h>
#include "engine/engine_array_safety.h"
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
@@ -96,7 +97,6 @@ static void islandColor(float rgba[4], int islanddofadr) {
// add contact-related geoms in mjvObject
static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
const mjvOption* vopt, mjvScene* scn) {
int body1, body2;
int objtype = mjOBJ_UNKNOWN, category = mjCAT_DECOR;
mjtNum mat[9], tmp[9], vec[3], frc[3], confrc[6], axis[3];
mjtNum framewidth, framelength, scl = m->stat.meansize;
@@ -147,13 +147,42 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// label contacting geom names or ids
if (vopt->label == mjLABEL_CONTACTPOINT) {
const char* name1 = mj_id2name(m, mjOBJ_GEOM, con->geom1);
char id1[10];
mjSNPRINTF(id1, "%d", con->geom1);
const char* name2 = mj_id2name(m, mjOBJ_GEOM, con->geom2);
char id2[10];
mjSNPRINTF(id2, "%d", con->geom2);
mjSNPRINTF(thisgeom->label, "%s | %s", name1 ? name1 : id1 , name2 ? name2 : id2);
char contactlabel[2][10];
for (int k=0; k<2; k++) {
// make geom label
if (con->geom[k]>=0) {
const char* geomname = mj_id2name(m, mjOBJ_GEOM, con->geom[k]);
if (geomname) {
mjSNPRINTF(contactlabel[k], "%s", geomname);
}
else {
mjSNPRINTF(contactlabel[k], "g%d", con->geom[k]);
}
}
// make flex elem or vert label
else {
const char* flexname = mj_id2name(m, mjOBJ_FLEX, con->flex[k]);
if (flexname) {
if (con->elem[k]>=0) {
mjSNPRINTF(contactlabel[k], "%s.e%d", flexname, con->elem[k]);
}
else {
mjSNPRINTF(contactlabel[k], "%s.v%d", flexname, con->vert[k]);
}
}
else {
if (con->elem[k]>=0) {
mjSNPRINTF(contactlabel[k], "f%d.e%d", con->flex[k], con->elem[k]);
}
else {
mjSNPRINTF(contactlabel[k], "f%d.v%d", con->flex[k], con->vert[k]);
}
}
}
}
mjSNPRINTF(thisgeom->label, "%s | %s", contactlabel[0], contactlabel[1]);
}
FINISH
@@ -234,12 +263,15 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// scale vector
mju_scl3(vec, vec, m->vis.map.force/m->stat.meanmass);
// get body ids
body1 = m->geom_bodyid[con->geom1];
body2 = m->geom_bodyid[con->geom2];
// get bodyflex ids
int bf[2];
for (int k=0; k<2; k++) {
bf[k] = (con->geom[k]>=0) ? m->geom_bodyid[con->geom[k]] :
m->nbody + con->flex[k];
}
// make sure arrow points towards body with higher id
if (body1 > body2) {
// make sure arrow points towards bodyflex with higher id
if (bf[0] > bf[1]) {
mju_scl3(vec, vec, -1);
}
@@ -249,8 +281,8 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
mjtNum to[3];
mju_add3(to, from, vec);
mjv_connector(thisgeom,
body1 > 0 && body2 > 0 && !split ? mjGEOM_ARROW2 : mjGEOM_ARROW,
m->vis.scale.forcewidth * scl,from, to);
bf[0] > 0 && bf[1] > 0 && !split ? mjGEOM_ARROW2 : mjGEOM_ARROW,
m->vis.scale.forcewidth * scl,from, to);
f2f(thisgeom->rgba, j == 2 ? m->vis.rgba.contactfriction : m->vis.rgba.contactforce, 4);
if (vopt->label == mjLABEL_CONTACTFORCE && j == (split ? 1 : 0)) {
mjSNPRINTF(thisgeom->label, "%-.3g", mju_norm3(frc));
@@ -477,12 +509,15 @@ static int bodycategory(const mjModel* m, int bodyid) {
// draw bounding box
static void drawBoundingBox(mjvGeom* thisgeom, mjData* d, mjvScene* scn,
static void drawBoundingBox(mjData* d, mjvScene* scn,
const mjtNum aabb[6], const mjtNum xpos[3],
const mjtNum xmat[9], const float rgba[4],
int i, int objtype, int category) {
const mjtNum xmat[9], const float rgba[4]) {
mjtNum x[3];
mjtNum dist[3][3];
mjvGeom* thisgeom;
int category = mjCAT_DECOR;
int objtype = mjOBJ_UNKNOWN;
int i = -1;
if (xmat != NULL) {
mju_rotVecMat(x, aabb, xmat);
@@ -558,49 +593,98 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
catmask &= (~mjCAT_STATIC);
}
// flex
objtype = mjOBJ_FLEX;
category = mjCAT_DYNAMIC;
if ((vopt->flags[mjVIS_FLEXVERT] || vopt->flags[mjVIS_FLEXEDGE] ||
vopt->flags[mjVIS_FLEXFACE] || vopt->flags[mjVIS_FLEXSKIN]) &&
(category & catmask)) {
for (int i=0; i<m->nflex; i++) {
if (vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[i]))]) {
START
// construct geom, pos = first vertex
mjv_initGeom(thisgeom, mjGEOM_FLEX, NULL,
d->flexvert_xpos + 3*m->flex_vertadr[i], NULL, NULL);
// size[0] = radius
thisgeom->size[0] = m->flex_radius[i];
// set material properties
setMaterial(m, thisgeom, m->flex_matid[i], m->flex_rgba+4*i, vopt->flags);
// set texcoord
if (m->flex_texcoordadr[i]>=0) {
thisgeom->texcoord = 1;
}
else {
thisgeom->texid = -1;
}
// glow flex if selected
if (pert->flexselect==i) {
markselected(&m->vis, thisgeom);
}
// skip if alpha is 0
if (thisgeom->rgba[3]==0) {
continue;
}
// vopt->label
if (vopt->label==mjLABEL_FLEX) {
makeLabel(m, mjOBJ_FLEX, i, thisgeom->label);
}
FINISH
}
}
}
// skin
objtype = mjOBJ_SKIN;
category = mjCAT_DYNAMIC;
if (vopt->flags[mjVIS_SKIN] && (category & catmask)) {
for (int i=0; i < m->nskin; i++) {
START
if (vopt->skingroup[mjMAX(0, mjMIN(mjNGROUP-1, m->skin_group[i]))]) {
START
// construct geom, pos = first bone
mjv_initGeom(thisgeom, mjGEOM_SKIN, NULL,
d->xpos + 3*m->skin_bonebodyid[m->skin_boneadr[i]], NULL, NULL);
// construct geom, pos = first bone
mjv_initGeom(thisgeom, mjGEOM_SKIN, NULL,
d->xpos + 3*m->skin_bonebodyid[m->skin_boneadr[i]], NULL, NULL);
// set material properties
setMaterial(m, thisgeom, m->skin_matid[i], m->skin_rgba+4*i, vopt->flags);
// set material properties
setMaterial(m, thisgeom, m->skin_matid[i], m->skin_rgba+4*i, vopt->flags);
// glow skin if selected
if (pert->skinselect == i) {
markselected(&m->vis, thisgeom);
// glow skin if selected
if (pert->skinselect == i) {
markselected(&m->vis, thisgeom);
}
// set texcoord
if (m->skin_texcoordadr[i] >= 0) {
thisgeom->texcoord = 1;
}
// skip if alpha is 0
if (thisgeom->rgba[3] == 0) {
continue;
}
// vopt->label
if (vopt->label == mjLABEL_SKIN) {
makeLabel(m, mjOBJ_SKIN, i, thisgeom->label);
}
FINISH
}
// set texcoord
if (m->skin_texcoordadr[i] >= 0) {
thisgeom->texcoord = 1;
}
// skip if alpha is 0
if (thisgeom->rgba[3] == 0) {
continue;
}
// vopt->label
if (vopt->label == mjLABEL_SKIN) {
makeLabel(m, mjOBJ_SKIN, i, thisgeom->label);
}
FINISH
}
}
// bounding volume hierarchy
if (vopt->flags[mjVIS_MIDPHASE]) {
int bodyid = 0;
// body BVH
if (vopt->flags[mjVIS_BODYBVH]) {
float rgba[] = {1, 0, 0, 1};
for (int i = 0; i < m->nbvh; i++) {
for (int i = 0; i < m->nbvhstatic; i++) {
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
if (scn->ngeom >= scn->maxgeom) break;
if (m->bvh_depth[i] != vopt->bvh_depth) {
@@ -610,7 +694,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
// find geom number
int geomid = m->bvh_geomid[i];
int bodyid = 0;
int geomid = m->bvh_nodeid[i];
while (i >= m->body_bvhadr[bodyid] + m->body_bvhnum[bodyid]) {
if (++bodyid >= m->nbody) {
break;
@@ -631,11 +716,41 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category);
drawBoundingBox(d, scn, aabb, xpos, xmat, rgba);
}
}
// mesh bounding volume hierarchy
// flex BVH
if (vopt->flags[mjVIS_FLEXBVH]) {
float rgba[] = {1, 0, 0, 0.1};
for (int f=0; f<m->nflex; f++) {
if (m->flex_bvhnum[f] &&
vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) {
for (int i=m->flex_bvhadr[f]; i<m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) {
int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1;
if (scn->ngeom >= scn->maxgeom) break;
if (m->bvh_depth[i] != vopt->bvh_depth) {
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
continue;
}
}
// set box data
mjtNum *aabb = d->bvh_aabb_dyn + 6*(i - m->nbvhstatic);
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
// b/304453879 : add LINEBOX geom for bounding box visualization
START
mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba);
FINISH
}
}
}
}
// mesh BVH
if (vopt->flags[mjVIS_MESHBVH]) {
float rgba[] = {1, 0, 0, 1};
for (int geomid = 0; geomid < m->ngeom; geomid++) {
@@ -666,7 +781,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category);
drawBoundingBox(d, scn, aabb, xpos, xmat, rgba);
}
}
}
@@ -2123,6 +2238,320 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn
}
// construct face, flat normals
static void makeFace(float* _face, float* _normal, mjtNum radius, const mjtNum* vertxpos,
int nface, int i0, int i1, int i2) {
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
const mjtNum* v2 = vertxpos + 3*i2;
// compute normal
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 nrm[3];
mju_cross(nrm, v01, v02);
mju_normalize3(nrm);
// set vertices: offset by radius*normal
mjtNum temp[3];
mju_addScl3(temp, v0, nrm, radius);
mju_n2f(face, temp, 3);
mju_addScl3(temp, v1, nrm, radius);
mju_n2f(face+3, temp, 3);
mju_addScl3(temp, v2, nrm, radius);
mju_n2f(face+6, temp, 3);
// set normals
mju_n2f(normal, nrm, 3);
mju_n2f(normal+3, nrm, 3);
mju_n2f(normal+6, nrm, 3);
}
// add face normal to vertices
static void addNormal(mjtNum* vertnorm, const mjtNum* vertxpos,
int i0, int i1, int i2) {
// compute normal*area
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
const mjtNum* v2 = vertxpos + 3*i2;
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 nrm[3];
mju_cross(nrm, v01, v02);
mju_normalize3(nrm);
// accumulate at each vertex
mju_addTo3(vertnorm + 3*i0, nrm);
mju_addTo3(vertnorm + 3*i1, nrm);
mju_addTo3(vertnorm + 3*i2, nrm);
}
// construct face, smooth normals
static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_flat,
const mjtNum* vertnorm, const mjtNum* vertxpos,
int nface, int i0, int i1, int i2) {
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
int ind[3] = {i0, i1, i2};
int sign = radius>0 ? 1 : -1;
// flat shading
if (flg_flat) {
// compute face normal
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
const mjtNum* v2 = vertxpos + 3*i2;
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 nrm[3];
mju_cross(nrm, v01, v02);
mju_normalize3(nrm);
// set all vertex normals equal to face normal
for (int k=0; k<3; k++){
normal[3*k+0] = (float) (sign*nrm[0]);
normal[3*k+1] = (float) (sign*nrm[1]);
normal[3*k+2] = (float) (sign*nrm[2]);
}
}
// smooth shading
else {
for (int k=0; k<3; k++){
normal[3*k+0] = (float) (sign*vertnorm[3*ind[k]+0]);
normal[3*k+1] = (float) (sign*vertnorm[3*ind[k]+1]);
normal[3*k+2] = (float) (sign*vertnorm[3*ind[k]+2]);
}
}
// set positions: vertices offset by radius*normal
for (int k=0; k<3; k++){
face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + radius*vertnorm[3*ind[k]+0]);
face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + radius*vertnorm[3*ind[k]+1]);
face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + radius*vertnorm[3*ind[k]+2]);
}
}
// construct side in 2D face
static void makeSide(float* _face, float* _normal, mjtNum radius,
const mjtNum* vertnorm, const mjtNum* vertxpos,
int nface, int i0, int i1) {
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
// compute normal
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, vertnorm+3*i1);
if (radius<0) {
mju_scl3(nrm, nrm, -1);
}
mju_normalize3(nrm);
// set normals
for (int k=0; k<3; k++){
normal[3*k+0] = (float) nrm[0];
normal[3*k+1] = (float) nrm[1];
normal[3*k+2] = (float) nrm[2];
}
// set positions
int ind[3] = {i0, i1, i1};
for (int k=0; k<3; k++){
mjtNum sign = (k==1 ? -1 : +1);
face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + sign*radius*vertnorm[3*ind[k]+0]);
face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + sign*radius*vertnorm[3*ind[k]+1]);
face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + sign*radius*vertnorm[3*ind[k]+2]);
}
}
// copy texcoord for face
static void copyTex(float* dst, const float* src, int nface, int i0, int i1, int i2) {
if (!dst || !src) {
return;
}
dst[6*nface+0] = src[2*i0];
dst[6*nface+1] = src[2*i0+1];
dst[6*nface+2] = src[2*i1];
dst[6*nface+3] = src[2*i1+1];
dst[6*nface+4] = src[2*i2];
dst[6*nface+5] = src[2*i2+1];
}
// update visible flexes only
void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) {
// save flex visualization flags in scene (needed by renderer)
scn->flexvertopt = opt->flags[mjVIS_FLEXVERT];
scn->flexedgeopt = opt->flags[mjVIS_FLEXEDGE];
scn->flexfaceopt = opt->flags[mjVIS_FLEXFACE];
scn->flexskinopt = opt->flags[mjVIS_FLEXSKIN];
// convert vertex positions from mjtNum to float
for (int v=0; v<3*m->nflexvert; v++) {
scn->flexvert[v] = (float) d->flexvert_xpos[v];
}
// construct faces
for (int f=0; f<m->nflex; f++) {
int dim = m->flex_dim[f];
mjtNum radius = m->flex_radius[f];
mjtByte flg_flat = m->flex_flatskin[f];
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
float* face = scn->flexface + 9*scn->flexfaceadr[f];
float* normal = scn->flexnormal + 9*scn->flexfaceadr[f];
float* texdst = m->flex_texcoordadr[f]>=0 ?
scn->flextexcoord + 6*scn->flexfaceadr[f] : NULL;
const float* texsrc = m->flex_texcoordadr[f]>=0 ?
m->flex_texcoord + 2*m->flex_texcoordadr[f] : NULL;
// 1D, or face and skin disabled: no faces
if (dim==1 || (!opt->flags[mjVIS_FLEXFACE] && !opt->flags[mjVIS_FLEXSKIN])) {
scn->flexfaceused[f] = 0;
}
// 2D or 3D face: faces from elements, flat normals, texture
else if (!opt->flags[mjVIS_FLEXSKIN]) {
int nface = 0;
for (int e=0; e<m->flex_elemnum[f]; e++) {
// in 3D, show only elements in selected layer
if (dim==2 || m->flex_elemlayer[m->flex_elemadr[f]+e]==opt->flex_layer) {
// get element data
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
// triangles: two faces per element
if (dim==2) {
makeFace(face, normal, radius, vertxpos, nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, edata[0], edata[1], edata[2]);
nface++;
makeFace(face, normal, radius, vertxpos, nface, edata[0], edata[2], edata[1]);
copyTex(texdst, texsrc, nface, edata[0], edata[2], edata[1]);
nface++;
}
// tetrahedra: four faces per element
else {
makeFace(face, normal, radius, vertxpos,
nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, edata[0], edata[1], edata[2]);
nface++;
makeFace(face, normal, radius, vertxpos,
nface, edata[0], edata[2], edata[3]);
copyTex(texdst, texsrc, nface, edata[0], edata[2], edata[3]);
nface++;
makeFace(face, normal, radius, vertxpos,
nface, edata[0], edata[3], edata[1]);
copyTex(texdst, texsrc, nface, edata[0], edata[3], edata[1]);
nface++;
makeFace(face, normal, radius, vertxpos,
nface, edata[1], edata[3], edata[2]);
copyTex(texdst, texsrc, nface, edata[1], edata[3], edata[2]);
nface++;
}
}
}
// save face count
scn->flexfaceused[f] = nface;
}
// 2D or 3D skin: faces from elements (2D) or shells (3D), smooth normals, texture
else {
// allocate and clear vertex normals for smoothing
mj_markStack(d);
mjtNum* vertnorm = mj_stackAllocNum(d, 3*m->flex_vertnum[f]);
mju_zero(vertnorm, 3*m->flex_vertnum[f]);
// add vertex normals: top element sides in 2D, shell fragments in 3D
if (dim==2) {
for (int e=0; e<m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
addNormal(vertnorm, vertxpos, edata[0], edata[1], edata[2]);
}
} else {
for (int s=0; s<m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
addNormal(vertnorm, vertxpos, sdata[0], sdata[1], sdata[2]);
}
}
// normalize vertex normals
for (int i=0; i<m->flex_vertnum[f]; i++) {
mju_normalize3(vertnorm+3*i);
}
// create faces, offset along smoothed vertex normals, and texcoord
int nface = 0;
if (dim==2) {
for (int e=0; e<m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos,
nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, edata[0], edata[1], edata[2]);
nface++;
makeSmooth(face, normal, -radius, flg_flat, vertnorm, vertxpos,
nface, edata[0], edata[2], edata[1]);
copyTex(texdst, texsrc, nface, edata[0], edata[2], edata[1]);
nface++;
}
} else {
for (int s=0; s<m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos,
nface, sdata[0], sdata[1], sdata[2]);
copyTex(texdst, texsrc, nface, sdata[0], sdata[1], sdata[2]);
nface++;
}
}
// 2D: close sides using shell fragments
if (dim==2) {
for (int s=0; s<m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
makeSide(face, normal, radius, vertnorm, vertxpos,
nface, sdata[0], sdata[1]);
copyTex(texdst, texsrc, nface, sdata[0], sdata[1], sdata[1]);
nface++;
makeSide(face, normal, -radius, vertnorm, vertxpos,
nface, sdata[1], sdata[0]);
copyTex(texdst, texsrc, nface, sdata[1], sdata[0], sdata[0]);
nface++;
}
}
// save face count
scn->flexfaceused[f] = nface;
mj_freeStack(d);
}
// check face count, SHOULD NOT OCCUR
if (scn->flexfaceused[f] > scn->flexfacenum[f]) {
mju_error("too many flex faces in mjv_updateActiveFlex");
}
}
}
// update all skins, here for backward API compatibility
void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
mjvOption opt;
@@ -2132,6 +2561,7 @@ void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
}
// update visible skins only
void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) {
// process skins
@@ -2146,7 +2576,8 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
memset(scn->skinvert + 3*vertadr, 0, 3*vertnum*sizeof(float));
memset(scn->skinnormal + 3*vertadr, 0, 3*vertnum*sizeof(float));
if (opt->skingroup[m->skin_group[i]]) {
// update only if visible
if (opt->skingroup[mjMAX(0, mjMIN(mjNGROUP-1, m->skin_group[i]))]) {
// accumulate positions from all bones
for (int j=m->skin_boneadr[i];
j < m->skin_boneadr[i]+m->skin_bonenum[i];
@@ -2234,10 +2665,10 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
// normalize normals
for (int k=vertadr; k < vertadr+vertnum; k++) {
float s = sqrtf(
scn->skinnormal[3*k]*scn->skinnormal[3*k] +
scn->skinnormal[3*k+1]*scn->skinnormal[3*k+1] +
scn->skinnormal[3*k+2]*scn->skinnormal[3*k+2]
);
scn->skinnormal[3*k]*scn->skinnormal[3*k] +
scn->skinnormal[3*k+1]*scn->skinnormal[3*k+1] +
scn->skinnormal[3*k+2]*scn->skinnormal[3*k+2]
);
float scl = 1/mjMAX(mjMINVAL, s);
scn->skinnormal[3*k] *= scl;
@@ -2291,6 +2722,12 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt,
// update camera
mjv_updateCamera(m, d, cam, scn);
// update flexes
if (opt->flags[mjVIS_FLEXVERT] || opt->flags[mjVIS_FLEXEDGE] ||
opt->flags[mjVIS_FLEXFACE] || opt->flags[mjVIS_FLEXSKIN]) {
mjv_updateActiveFlex(m, d, scn, opt);
}
// update skins
if (opt->flags[mjVIS_SKIN]) {
mjv_updateActiveSkin(m, d, scn, opt);
src/engine/engine_vis_visualize.h
+3
View File
@@ -53,6 +53,9 @@ MJAPI void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn);
// update camera only
MJAPI void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn);
// update visible flexes only
MJAPI void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt);
// update skins only
MJAPI void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn);
src/render/render_gl3.c
+81 -2
View File
@@ -284,9 +284,9 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
}
}
// apply coordinate transformation, except for skin which is global
// apply coordinate transformation, except for flex and skin which are global
glPushMatrix();
if (geom->type != mjGEOM_SKIN) {
if (geom->type != mjGEOM_FLEX && geom->type != mjGEOM_SKIN) {
glTranslatef(geom->pos[0], geom->pos[1], geom->pos[2]);
glMultMatrixf(mat);
}
@@ -412,6 +412,85 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
glEnd();
break;
case mjGEOM_FLEX: // flex
// no texture for vertices and edges
if (glIsEnabled(GL_TEXTURE_2D)) {
glDisable(GL_TEXTURE_2D);
}
// vertex spheres
if (size[0]>0 && scn->flexvertopt &&
!(scn->flexskinopt && scn->flexfaceused[geom->objid])) {
for (int v=scn->flexvertadr[geom->objid];
v<scn->flexvertadr[geom->objid]+scn->flexvertnum[geom->objid]; v++) {
glPushMatrix();
glTranslatef(scn->flexvert[3*v], scn->flexvert[3*v+1], scn->flexvert[3*v+2]);
glScalef(size[0], size[0], size[0]);
glCallList(con->baseBuiltin + mjrSPHERE);
glPopMatrix();
}
}
// edge cylinders
if (size[0]>0 && scn->flexedgeopt &&
!(scn->flexskinopt && scn->flexfaceused[geom->objid])) {
for (int e=scn->flexedgeadr[geom->objid];
e<scn->flexedgeadr[geom->objid]+scn->flexedgenum[geom->objid]; e++) {
// get vertices for this edge
float* v1 = scn->flexvert + 3*(scn->flexvertadr[geom->objid]+scn->flexedge[2*e]);
float* v2 = scn->flexvert + 3*(scn->flexvertadr[geom->objid]+scn->flexedge[2*e+1]);
// compute legth and rotation matrix
mjtNum vec[3] = {v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]};
mjtNum len = mju_normalize3(vec);
mjtNum edgequat[4], edgemat[9];
mju_quatZ2Vec(edgequat, vec);
mju_negQuat(edgequat, edgequat);
mju_quat2Mat(edgemat, edgequat);
mat[0] = (float)edgemat[0];
mat[1] = (float)edgemat[1];
mat[2] = (float)edgemat[2];
mat[4] = (float)edgemat[3];
mat[5] = (float)edgemat[4];
mat[6] = (float)edgemat[5];
mat[8] = (float)edgemat[6];
mat[9] = (float)edgemat[7];
mat[10] = (float)edgemat[8];
// transform and render
glPushMatrix();
glTranslatef((v1[0]+v2[0])*0.5f, (v1[1]+v2[1])*0.5f, (v1[2]+v2[2])*0.5f);
glMultMatrixf(mat);
glScalef(size[0], size[0], (float)(len*0.5));
glCallList(con->baseBuiltin + mjrCYLINDEROPEN);
glPopMatrix();
}
}
// restore texture for faces
if (glIsEnabled(GL_TEXTURE_2D)) {
glEnable(GL_TEXTURE_2D);
}
// face triangles
if (scn->flexfaceused[geom->objid]) {
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_NORMAL_ARRAY);
glVertexPointer(3, GL_FLOAT, 0, scn->flexface + 9*scn->flexfaceadr[geom->objid]);
glNormalPointer(GL_FLOAT, 0, scn->flexnormal + 9*scn->flexfaceadr[geom->objid]);
if (geom->texcoord && geom->texid>=0) {
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
glTexCoordPointer(2, GL_FLOAT, 0, scn->flextexcoord + 6*scn->flexfaceadr[geom->objid]);
}
glDrawArrays(GL_TRIANGLES, 0, 3*scn->flexfaceused[geom->objid]);
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_NORMAL_ARRAY);
if (geom->texcoord && geom->texid>=0) {
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
}
}
break;
case mjGEOM_SKIN: // skin
// vertex positions
glEnableClientState(GL_VERTEX_ARRAY);
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;
src/xml/xml_base.h
+1
View File
@@ -70,6 +70,7 @@ extern const mjMap bias_map[];
extern const mjMap stage_map[];
extern const mjMap datatype_map[];
extern const mjMap meshtype_map[];
extern const mjMap flexself_map[];
//---------------------------------- Base XML class ------------------------------------------------
src/xml/xml_native_reader.cc
+298 -11
View File
@@ -34,6 +34,7 @@
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_composite.h"
#include "user/user_flexcomp.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
@@ -77,7 +78,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 204;
static const int nMJCF = 223;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -92,10 +93,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"inttotal", "interval", "tolrange"},
{">"},
{"option", "*", "25",
{"option", "*", "26",
"timestep", "apirate", "impratio", "tolerance", "ls_tolerance", "noslip_tolerance",
"mpr_tolerance", "gravity", "wind", "magnetic", "density", "viscosity",
"o_margin", "o_solref", "o_solimp",
"o_margin", "o_solref", "o_solimp", "o_friction",
"integrator", "cone", "jacobian",
"solver", "iterations", "ls_iterations", "noslip_iterations", "mpr_iterations",
"sdf_iterations", "sdf_initpoints"},
@@ -112,8 +113,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"visual", "*", "0"},
{"<"},
{"global", "?", "10", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow", "offwidth",
"offheight", "realtime", "ellipsoidinertia"},
{"global", "?", "10", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow",
"offwidth", "offheight", "realtime", "ellipsoidinertia"},
{"quality", "?", "5", "shadowsize", "offsamples", "numslices", "numstacks",
"numquads"},
{"headlight", "?", "4", "ambient", "diffuse", "specular", "active"},
@@ -292,6 +293,32 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{"flexcomp", "*", "26", "name", "class", "type", "dim", "flatskin",
"count", "spacing", "radius", "rigid", "mass", "inertiabox",
"scale", "file", "point", "element", "texcoord", "material", "rgba", "selfcollide",
"flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler"},
{"<"},
{"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"},
{"contact", "?", "10", "contype", "conaffinity", "condim", "priority",
"friction", "solmix", "solref", "solimp", "margin", "gap", },
{"pin", "*", "4", "id", "range", "grid", "gridrange"},
{">"},
{">"},
{"deformable", "*", "0"},
{"<"},
{"flex", "*", "12", "name", "group", "dim", "radius", "material", "rgba", "flatskin",
"selfcollide", "body", "vertex", "element", "texcoord"},
{"<"},
{"contact", "?", "10", "contype", "conaffinity", "condim", "priority",
"friction", "solmix", "solref", "solimp", "margin", "gap"},
{"edge", "?", "2", "stiffness", "damping"},
{">"},
{"skin", "*", "9", "name", "file", "material", "rgba", "inflate",
"vertex", "texcoord", "face", "group"},
{"<"},
{"bone", "*", "5", "body", "bindpos", "bindquat", "vertid", "vertweight"},
{">"},
{">"},
{"contact", "*", "0"},
@@ -311,6 +338,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"active", "solref", "solimp"},
{"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef",
"active", "solref", "solimp"},
{"flex", "*", "6", "name", "class", "flex",
"active", "solref", "solimp"},
{">"},
{"tendon", "*", "0"},
@@ -561,12 +590,13 @@ const mjMap solver_map[solver_sz] = {
// constraint type
const int equality_sz = 5;
const int equality_sz = 6;
const mjMap equality_map[equality_sz] = {
{"connect", mjEQ_CONNECT},
{"weld", mjEQ_WELD},
{"joint", mjEQ_JOINT},
{"tendon", mjEQ_TENDON},
{"flex", mjEQ_FLEX},
{"distance", mjEQ_DISTANCE}
};
@@ -654,7 +684,7 @@ const mjMap datatype_map[datatype_sz] = {
// LR mode
const int lrmode_sz = 4;
const mjMap lrmode_map[datatype_sz] = {
const mjMap lrmode_map[lrmode_sz] = {
{"none", mjLRMODE_NONE},
{"muscle", mjLRMODE_MUSCLE},
{"muscleuser", mjLRMODE_MUSCLEUSER},
@@ -702,12 +732,35 @@ const mjMap tkind_map[2] = {
// mesh type
const mjMap meshtype_map[2] = {
const mjMap meshtype_map[2] = {
{"false", mjVOLUME_MESH},
{"true", mjSHELL_MESH},
};
// flexcomp type
const mjMap fcomp_map[mjNFCOMPTYPES] = {
{"grid", mjFCOMPTYPE_GRID},
{"box", mjFCOMPTYPE_BOX},
{"cylinder", mjFCOMPTYPE_CYLINDER},
{"ellipsoid", mjFCOMPTYPE_ELLIPSOID},
{"mesh", mjFCOMPTYPE_MESH},
{"gmsh", mjFCOMPTYPE_GMSH},
{"direct", mjFCOMPTYPE_DIRECT}
};
// flex selfcollide type
const mjMap flexself_map[5] = {
{"none", mjFLEXSELF_NONE},
{"narrow", mjFLEXSELF_NARROW},
{"bvh", mjFLEXSELF_BVH},
{"sap", mjFLEXSELF_SAP},
{"auto", mjFLEXSELF_AUTO},
};
//---------------------------------- class mjXReader implementation --------------------------------
// constructor
@@ -821,6 +874,11 @@ void mjXReader::Parse(XMLElement* root) {
Contact(section);
}
for (XMLElement* section = root->FirstChildElement("deformable"); section;
section = section->NextSiblingElement("deformable")) {
Deformable(section);
}
for (XMLElement* section = root->FirstChildElement("equality"); section;
section = section->NextSiblingElement("equality")) {
Equality(section);
@@ -959,6 +1017,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
ReadAttr(section, "o_margin", 1, &opt->o_margin, text);
ReadAttr(section, "o_solref", mjNREF, opt->o_solref, text, false, false);
ReadAttr(section, "o_solimp", mjNIMP, opt->o_solimp, text, false, false);
ReadAttr(section, "o_friction", 5, opt->o_friction, text, false, false);
MapValue(section, "integrator", &opt->integrator, integrator_map, integrator_sz);
MapValue(section, "cone", &opt->cone, cone_map, cone_sz);
@@ -1190,6 +1249,68 @@ void mjXReader::Statistic(XMLElement* section) {
//---------------------------------- one-element parsers -------------------------------------------
// flex element parser
void mjXReader::OneFlex(XMLElement* elem, mjCFlex* pflex) {
string text;
int n;
// read attributes
ReadAttrTxt(elem, "name", pflex->name);
ReadAttr(elem, "radius", 1, &pflex->radius, text);
ReadAttrTxt(elem, "material", pflex->material);
ReadAttr(elem, "rgba", 4, pflex->rgba, text);
if (MapValue(elem, "flatskin", &n, bool_map, 2)) {
pflex->flatskin = (n==1);
}
ReadAttrInt(elem, "dim", &pflex->dim);
ReadAttrInt(elem, "group", &pflex->group);
// read data vectors
if (ReadAttrTxt(elem, "body", text, true)) {
String2Vector(text, pflex->vertbody);
}
if (ReadAttrTxt(elem, "vertex", text)) {
String2Vector(text, pflex->vert);
}
if (ReadAttrTxt(elem, "element", text, true)) {
String2Vector(text, pflex->elem);
}
if (ReadAttrTxt(elem, "texcoord", text)) {
String2Vector(text, pflex->texcoord);
}
// contact subelement
XMLElement* cont = elem->FirstChildElement("contact");
if (cont) {
ReadAttrInt(cont, "contype", &pflex->contype);
ReadAttrInt(cont, "conaffinity", &pflex->conaffinity);
ReadAttrInt(cont, "condim", &pflex->condim);
ReadAttrInt(cont, "priority", &pflex->priority);
ReadAttr(cont, "friction", 3, pflex->friction, text, false, false);
ReadAttr(cont, "solmix", 1, &pflex->solmix, text);
ReadAttr(cont, "solref", mjNREF, pflex->solref, text, false, false);
ReadAttr(cont, "solimp", mjNIMP, pflex->solimp, text, false, false);
ReadAttr(cont, "margin", 1, &pflex->margin, text);
ReadAttr(cont, "gap", 1, &pflex->gap, text);
if (MapValue(cont, "internal", &n, bool_map, 2)) {
pflex->internal = (n==1);
}
MapValue(cont, "selfcollide", &pflex->selfcollide, flexself_map, 5);
ReadAttrInt(cont, "activelayers", &pflex->activelayers);
}
// edge subelement
XMLElement* edge = elem->FirstChildElement("edge");
if (edge) {
ReadAttr(edge, "stiffness", 1, &pflex->edgestiffness, text);
ReadAttr(edge, "damping", 1, &pflex->edgedamping, text);
}
GetXMLPos(elem, pflex);
}
// mesh element parser
void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) {
int n;
@@ -1596,6 +1717,10 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
ReadAttr(elem, "polycoef", 5, pequality->data, text);
break;
case mjEQ_FLEX:
ReadAttrTxt(elem, "flex", pequality->name1, true);
break;
case mjEQ_DISTANCE:
throw mjXError(elem, "support for distance equality constraints was removed in MuJoCo 2.2.2");
break;
@@ -2136,6 +2261,124 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
// make flexcomp
void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) {
string text;
int n;
// create out-of-DOM element
mjCFlexcomp fcomp;
// common properties
ReadAttrTxt(elem, "name", fcomp.name, true);
if (MapValue(elem, "type", &n, fcomp_map, mjNFCOMPTYPES)) {
fcomp.type = (mjtFcompType)n;
}
ReadAttr(elem, "count", 3, fcomp.count, text);
ReadAttr(elem, "spacing", 3, fcomp.spacing, text);
ReadAttr(elem, "scale", 3, fcomp.scale, text);
ReadAttr(elem, "mass", 1, &fcomp.mass, text);
ReadAttr(elem, "inertiabox", 1, &fcomp.inertiabox, text);
ReadAttrTxt(elem, "file", fcomp.file);
ReadAttrTxt(elem, "material", fcomp.def.flex.material);
ReadAttr(elem, "rgba", 4, fcomp.def.flex.rgba, text);
if (MapValue(elem, "flatskin", &n, bool_map, 2)) {
fcomp.def.flex.flatskin = (n==1);
}
ReadAttrInt(elem, "dim", &fcomp.def.flex.dim);
ReadAttr(elem, "radius", 1, &fcomp.def.flex.radius, text);
ReadAttrInt(elem, "group", &fcomp.def.flex.group);
// pose
ReadAttr(elem, "pos", 3, fcomp.pos, text);
ReadAttr(elem, "quat", 4, fcomp.quat, text);
ReadAlternative(elem, fcomp.alt);
// user or internal
if (MapValue(elem, "rigid", &n, bool_map, 2)) {
fcomp.rigid = (n==1);
}
if (ReadAttrTxt(elem, "point", text)){
String2Vector(text, fcomp.point);
}
if (ReadAttrTxt(elem, "element", text)){
String2Vector(text, fcomp.element);
}
if (ReadAttrTxt(elem, "texcoord", text)) {
String2Vector(text, fcomp.texcoord);
}
// edge
XMLElement* edge = elem->FirstChildElement("edge");
if (edge) {
if (MapValue(edge, "equality", &n, bool_map, 2)) {
fcomp.equality = (n==1);
}
ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.solref, text, false, false);
ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.solimp, text, false, false);
ReadAttr(edge, "stiffness", 1, &fcomp.def.flex.edgestiffness, text);
ReadAttr(edge, "damping", 1, &fcomp.def.flex.edgedamping, text);
}
// contact
XMLElement* cont = elem->FirstChildElement("contact");
if (cont) {
ReadAttrInt(cont, "contype", &fcomp.def.flex.contype);
ReadAttrInt(cont, "conaffinity", &fcomp.def.flex.conaffinity);
ReadAttrInt(cont, "condim", &fcomp.def.flex.condim);
ReadAttrInt(cont, "priority", &fcomp.def.flex.priority);
ReadAttr(cont, "friction", 3, fcomp.def.flex.friction, text, false, false);
ReadAttr(cont, "solmix", 1, &fcomp.def.flex.solmix, text);
ReadAttr(cont, "solref", mjNREF, fcomp.def.flex.solref, text, false, false);
ReadAttr(cont, "solimp", mjNIMP, fcomp.def.flex.solimp, text, false, false);
ReadAttr(cont, "margin", 1, &fcomp.def.flex.margin, text);
ReadAttr(cont, "gap", 1, &fcomp.def.flex.gap, text);
if (MapValue(cont, "internal", &n, bool_map, 2)) {
fcomp.def.flex.internal = (n==1);
}
MapValue(cont, "selfcollide", &fcomp.def.flex.selfcollide, flexself_map, 5);
ReadAttrInt(cont, "activelayers", &fcomp.def.flex.activelayers);
}
// pin
XMLElement* epin = elem->FirstChildElement("pin");
while (epin) {
// accumulate id, coord, range
vector<int> temp;
if (ReadAttrTxt(epin, "id", text)){
String2Vector(text, temp);
fcomp.pinid.insert(fcomp.pinid.end(), temp.begin(), temp.end());
}
if (ReadAttrTxt(epin, "range", text)){
String2Vector(text, temp);
fcomp.pinrange.insert(fcomp.pinrange.end(), temp.begin(), temp.end());
}
if (ReadAttrTxt(epin, "grid", text)){
String2Vector(text, temp);
fcomp.pingrid.insert(fcomp.pingrid.end(), temp.begin(), temp.end());
}
if (ReadAttrTxt(epin, "gridrange", text)){
String2Vector(text, temp);
fcomp.pingridrange.insert(fcomp.pingridrange.end(), temp.begin(), temp.end());
}
// advance
epin = epin->NextSiblingElement("pin");
}
// make flexcomp
char error[200];
bool res = fcomp.Make(pbody->model, pbody, error, 200);
// throw error
if (!res) {
throw mjXError(elem, error);
}
}
// add plugin
void mjXReader::OnePlugin(XMLElement* elem, mjCBase* object) {
object->is_plugin = true;
ReadAttrTxt(elem, "plugin", object->plugin_name);
@@ -2623,9 +2866,9 @@ void mjXReader::Asset(XMLElement* section) {
OneMesh(elem, pmesh);
}
// skin sub-element
// skin sub-element... deprecate ???
else if (name=="skin") {
// create mesh and parse
// create skin and parse
mjCSkin* pskin = model->AddSkin();
OneSkin(elem, pskin);
}
@@ -2771,10 +3014,16 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
// composite sub-element
else if (name=="composite") {
// create composite and parse
// parse composite
OneComposite(elem, pbody, def);
}
// flexcomp sub-element
else if (name=="flexcomp") {
// parse flexcomp
OneFlexcomp(elem, pbody);
}
// body sub-element
else if (name=="body") {
// read childdef
@@ -2888,6 +3137,44 @@ void mjXReader::Equality(XMLElement* section) {
// deformable section parser
void mjXReader::Deformable(XMLElement* section) {
string name;
XMLElement* elem;
// iterate over child elements
elem = section->FirstChildElement();
while (elem) {
// get sub-element name
name = elem->Value();
// get class if specified, otherwise use default0
mjCDef* def = GetClass(elem);
if (!def) {
def = model->defaults[0];
}
// flex sub-element
if (name=="flex") {
// create flex and parse
mjCFlex* pflex = model->AddFlex();
OneFlex(elem, pflex);
}
// skin sub-element
else if (name=="skin") {
// create skin and parse
mjCSkin* pskin = model->AddSkin();
OneSkin(elem, pskin);
}
// advance to next element
elem = elem->NextSiblingElement();
}
}
// tendon section parser
void mjXReader::Tendon(XMLElement* section) {
string text, text1;
src/xml/xml_native_reader.h
+3
View File
@@ -44,6 +44,7 @@ class mjXReader : public mjXBase {
void Asset(tinyxml2::XMLElement* section); // asset section
void Body(tinyxml2::XMLElement* section, mjCBody* pbody); // body/world section
void Contact(tinyxml2::XMLElement* section); // contact section
void Deformable(tinyxml2::XMLElement* section); // deformable section
void Equality(tinyxml2::XMLElement* section); // equality section
void Tendon(tinyxml2::XMLElement* section); // tendon section
void Actuator(tinyxml2::XMLElement* section); // actuator section
@@ -51,6 +52,7 @@ class mjXReader : public mjXBase {
void Keyframe(tinyxml2::XMLElement* section); // keyframe section
// single element parsers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh);
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial);
@@ -64,6 +66,7 @@ class mjXReader : public mjXBase {
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon);
void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator);
void OneComposite(tinyxml2::XMLElement* elem, mjCBody* pbody, mjCDef* def);
void OneFlexcomp(tinyxml2::XMLElement* elem, mjCBody* pbody);
void OnePlugin(tinyxml2::XMLElement* elem, mjCBase* object);
mjXSchema schema; // schema used for validation
src/xml/xml_native_writer.cc
+116 -16
View File
@@ -82,6 +82,74 @@ XMLElement* mjXWriter::InsertEnd(XMLElement* parent, const char* name) {
//---------------------------------- class mjXWriter: one-element writers --------------------------
// write flex
void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) {
string text;
mjCFlex defflex;
// common attributes
WriteAttrTxt(elem, "name", pflex->name);
WriteAttr(elem, "radius", 1, &pflex->radius, &defflex.radius);
if (pflex->material != defflex.material) {
WriteAttrTxt(elem, "material", pflex->material);
}
WriteAttr(elem, "rgba", 4, pflex->rgba, defflex.rgba);
WriteAttrKey(elem, "flatskin", bool_map, 2, pflex->flatskin, defflex.flatskin);
WriteAttrInt(elem, "dim", pflex->dim, defflex.dim);
WriteAttrInt(elem, "group", pflex->group, defflex.group);
// data vectors
if (!pflex->vertbody.empty()) {
Vector2String(text, pflex->vertbody);
WriteAttrTxt(elem, "body", text);
}
if (!pflex->vert.empty()) {
Vector2String(text, pflex->vert);
WriteAttrTxt(elem, "vertex", text);
}
if (!pflex->elem.empty()) {
Vector2String(text, pflex->elem);
WriteAttrTxt(elem, "element", text);
}
if (!pflex->texcoord.empty()) {
Vector2String(text, pflex->texcoord);
WriteAttrTxt(elem, "texcoord", text);
}
// contact subelement
XMLElement* cont = InsertEnd(elem, "contact");
WriteAttrInt(cont, "contype", pflex->contype, defflex.contype);
WriteAttrInt(cont, "conaffinity", pflex->conaffinity, defflex.conaffinity);
WriteAttrInt(cont, "condim", pflex->condim, defflex.condim);
WriteAttrInt(cont, "priority", pflex->priority, defflex.priority);
WriteAttr(cont, "friction", 3, pflex->friction, defflex.friction);
WriteAttr(cont, "solmix", 1, &pflex->solmix, &defflex.solmix);
WriteAttr(cont, "solref", mjNREF, pflex->solref, defflex.solref);
WriteAttr(cont, "solimp", mjNIMP, pflex->solimp, defflex.solimp);
WriteAttr(cont, "margin", 1, &pflex->margin, &defflex.margin);
WriteAttr(cont, "gap", 1, &pflex->gap, &defflex.gap);
WriteAttrKey(cont, "internal", bool_map, 2, pflex->internal, defflex.internal);
WriteAttrKey(cont, "selfcollide", flexself_map, 5, pflex->selfcollide, defflex.selfcollide);
WriteAttrInt(cont, "activelayers", pflex->activelayers, defflex.activelayers);
// remove contact is no attributes
if (!cont->FirstAttribute()) {
elem->DeleteChild(cont);
}
// edge subelement
XMLElement* edge = InsertEnd(elem, "edge");
WriteAttr(edge, "stiffness", 1, &pflex->edgestiffness, &defflex.edgestiffness);
WriteAttr(edge, "damping", 1, &pflex->edgedamping, &defflex.edgedamping);
// remove edge if no attributes
if (!edge->FirstAttribute()) {
elem->DeleteChild(edge);
}
}
// write mesh
void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) {
string text;
@@ -221,12 +289,12 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
// special handling of limits
bool range_defined = pjoint->range[0]!=0 || pjoint->range[1]!=0;
bool limited_inferred = def->joint.limited==2 && pjoint->limited==range_defined;
bool limited_inferred = def->joint.limited==2 && pjoint->limited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited);
}
bool afrange_defined = pjoint->actfrcrange[0]!=0 || pjoint->actfrcrange[1]!=0;
bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==afrange_defined;
bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==(int)afrange_defined;
if (writingdefaults || !aflimited_inferred) {
WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3,
pjoint->actfrclimited, def->joint.actfrclimited);
@@ -515,6 +583,10 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
WriteAttr(elem, "polycoef", 5, peq->data);
break;
case mjEQ_FLEX:
WriteAttrTxt(elem, "flex", peq->name1);
break;
default:
mju_error("mjXWriter: unknown equality type.");
}
@@ -540,7 +612,7 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
// special handling of limits
bool range_defined = pten->range[0]!=0 || pten->range[1]!=0;
bool limited_inferred = def->tendon.limited==2 && pten->limited==range_defined;
bool limited_inferred = def->tendon.limited==2 && pten->limited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->tendon.limited);
}
@@ -624,17 +696,17 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
// special handling of limits
bool range_defined, limited_inferred;
range_defined = pact->ctrlrange[0]!=0 || pact->ctrlrange[1]!=0;
limited_inferred = def->actuator.ctrllimited==2 && pact->ctrllimited==range_defined;
limited_inferred = def->actuator.ctrllimited==2 && pact->ctrllimited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->actuator.ctrllimited);
}
range_defined = pact->forcerange[0]!=0 || pact->forcerange[1]!=0;
limited_inferred = def->actuator.forcelimited==2 && pact->forcelimited==range_defined;
limited_inferred = def->actuator.forcelimited==2 && pact->forcelimited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->actuator.forcelimited);
}
range_defined = pact->actrange[0]!=0 || pact->actrange[1]!=0;
limited_inferred = def->actuator.actlimited==2 && pact->actlimited==range_defined;
limited_inferred = def->actuator.actlimited==2 && pact->actlimited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->actuator.actlimited);
}
@@ -750,6 +822,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
Asset(root);
Body(InsertEnd(root, "worldbody"), model->GetWorld());
Contact(root);
Deformable(root);
Equality(root);
Tendon(root);
Actuator(root);
@@ -819,6 +892,7 @@ void mjXWriter::Option(XMLElement* root) {
WriteAttr(section, "o_margin", 1, &model->option.o_margin, &opt.o_margin);
WriteAttr(section, "o_solref", mjNREF, model->option.o_solref, opt.o_solref);
WriteAttr(section, "o_solimp", mjNIMP, model->option.o_solimp, opt.o_solimp);
WriteAttr(section, "o_friction", 5, model->option.o_friction, opt.o_friction);
WriteAttrKey(section, "integrator", integrator_map, integrator_sz,
model->option.integrator, opt.integrator);
@@ -1261,11 +1335,10 @@ void mjXWriter::Asset(XMLElement* root) {
int ntex = model->NumObjects(mjOBJ_TEXTURE);
int nmat = model->NumObjects(mjOBJ_MATERIAL);
int nmesh = model->NumObjects(mjOBJ_MESH);
int nskin = model->NumObjects(mjOBJ_SKIN);
int nhfield = model->NumObjects(mjOBJ_HFIELD);
// return if empty
if (ntex==0 && nmat==0 && nmesh==0 && nhfield==0 && nskin==0) {
if (ntex==0 && nmat==0 && nmesh==0 && nhfield==0) {
return;
}
@@ -1346,14 +1419,6 @@ void mjXWriter::Asset(XMLElement* root) {
}
}
// write skins
for (int i=0; i<nskin; i++) {
// create element and write
mjCSkin* pskin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
elem = InsertEnd(section, "skin");
OneSkin(elem, pskin);
}
// write hfields
for (int i=0; i<nhfield; i++) {
// create element
@@ -1513,6 +1578,41 @@ void mjXWriter::Equality(XMLElement* root) {
// deformable section
void mjXWriter::Deformable(XMLElement* root) {
XMLElement* elem;
// get sizes
int nflex = model->NumObjects(mjOBJ_FLEX);
int nskin = model->NumObjects(mjOBJ_SKIN);
// return if empty
if (nflex==0 && nskin==0) {
return;
}
// create section
XMLElement* section = InsertEnd(root, "deformable");
// write flexes
for (int i=0; i<nflex; i++) {
// create element and write
mjCFlex* pflex = (mjCFlex*)model->GetObject(mjOBJ_FLEX, i);
elem = InsertEnd(section, "flex");
OneFlex(elem, pflex);
}
// write skins
for (int i=0; i<nskin; i++) {
// create element and write
mjCSkin* pskin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
elem = InsertEnd(section, "skin");
OneSkin(elem, pskin);
}
}
// tendon section
void mjXWriter::Tendon(XMLElement* root) {
// skip section if empty
src/xml/xml_native_writer.h
+3 -1
View File
@@ -45,13 +45,15 @@ class mjXWriter : public mjXBase {
void Asset(tinyxml2::XMLElement* root); // asset section
void Body(tinyxml2::XMLElement* elem, mjCBody* body); // body/world section
void Contact(tinyxml2::XMLElement* root); // contact section
void Equality(tinyxml2::XMLElement* root); // equality constraint section
void Deformable(tinyxml2::XMLElement* root); // deformable section
void Equality(tinyxml2::XMLElement* root); // equality section
void Tendon(tinyxml2::XMLElement* root); // tendon section
void Actuator(tinyxml2::XMLElement* root); // actuator section
void Sensor(tinyxml2::XMLElement* root); // sensor section
void Keyframe(tinyxml2::XMLElement* root); // keyframe section
// single element writers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh, mjCDef* def);
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial, mjCDef* def);
src/xml/xml_util.cc
+68
View File
@@ -767,6 +767,42 @@ bool mjXUtil::ReadAttrInt(XMLElement* elem, const char* attr, int* data, bool re
// read vector<string> from string
void mjXUtil::String2Vector(const string& txt, vector<string>& vec) {
stringstream strm(txt);
vec.clear();
while (!strm.eof()) {
string word;
strm >> word;
if (strm.fail()) {
break;
} else {
vec.push_back(word);
}
}
}
// read vector<mjtNum> from string
void mjXUtil::String2Vector(const string& txt, vector<double>& vec) {
stringstream strm(txt);
vec.clear();
while (!strm.eof()) {
double num;
strm >> num;
if (strm.fail()) {
break;
} else {
vec.push_back(num);
}
}
}
// read vector<float> from string
void mjXUtil::String2Vector(const string& txt, vector<float>& vec) {
stringstream strm(txt);
@@ -803,6 +839,38 @@ void mjXUtil::String2Vector(const string& txt, vector<int>& vec) {
// write vector<float> to string
void mjXUtil::Vector2String(string& txt, const vector<string>& vec) {
stringstream strm;
for (size_t i=0; i<vec.size(); i++) {
if (i>0) {
strm << " ";
}
strm << vec[i];
}
txt = strm.str();
}
// write vector<double> to string
void mjXUtil::Vector2String(string& txt, const vector<double>& vec) {
stringstream strm;
for (size_t i=0; i<vec.size(); i++) {
if (i>0) {
strm << " ";
}
strm << vec[i];
}
txt = strm.str();
}
// write vector<float> to string
void mjXUtil::Vector2String(string& txt, const vector<float>& vec) {
stringstream strm;
src/xml/xml_util.h
+12
View File
@@ -160,12 +160,24 @@ class mjXUtil {
static bool ReadAttrInt(tinyxml2::XMLElement* elem, const char* attr, int* data,
bool required = false);
// read vector<string> from string
static void String2Vector(const std::string& txt, std::vector<std::string>& vec);
// read vector<double> from string
static void String2Vector(const std::string& txt, std::vector<double>& vec);
// read vector<float> from string
static void String2Vector(const std::string& txt, std::vector<float>& vec);
// read vector<int> from string
static void String2Vector(const std::string& txt, std::vector<int>& vec);
// write vector<string> to string
static void Vector2String(std::string& txt, const std::vector<std::string>& vec);
// write vector<double> to string
static void Vector2String(std::string& txt, const std::vector<double>& vec);
// write vector<float> to string
static void Vector2String(std::string& txt, const std::vector<float>& vec);