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);