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

Added a collision midphase on geom pairs using rotated AABBs static trees.

Browse Source 
PiperOrigin-RevId: 517357695
Change-Id: I068fee714d9638be2fb6d042d5d86f8ff970635b
This commit is contained in:
Alessio Quaglino
2023-03-17 02:36:38 -07:00
committed by Copybara-Service
parent 47b58f5ccb
commit 70959c1a2c
30 changed files with 2228 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+6
View File
@@ -705,6 +705,12 @@ from its default.
function. If a new contact is detected it is added, allowing for up to 4 additional contact points. This feature is function. If a new contact is detected it is added, allowing for up to 4 additional contact points. This feature is
currently considered experimental, and both the behavior and the way it is activated may change in the future. currently considered experimental, and both the behavior and the way it is activated may change in the future.
.. _option-flag-midphase:
:at:`midphase`: :at-val:`[disable, enable], "enable"`
This flag disables the mid-phase collision filtering using a static AABB bounding volume hierarchy (a BVH binary
tree). If disabled, all geoms pairs that are allowed to collide are checked for collisions.
.. _size: .. _size:
doc/changelog.rst
+10
View File
@@ -30,6 +30,16 @@ General
one step (ms), 0.5, 0.53, 0.77, 5.0 one step (ms), 0.5, 0.53, 0.77, 5.0
steps/second, 2000, 1900, 1300, 200 steps/second, 2000, 1900, 1300, 200
- Added a collision mid-phase for pruning geoms in body pairs, see :ref:`documentation<coSelection>` for more details.
This is based on static AABB bounding volume hierarchy (a BVH binary tree) in the body inertial frame.
.. image:: images/computation/midphase.gif
:width: 450px
.. youtube:: e0babIM8hBo
:align: right
:height: 150px
- The ``mjd_transitionFD`` function no longer triggers sensor calculation unless explicitly requested. - The ``mjd_transitionFD`` function no longer triggers sensor calculation unless explicitly requested.
- Corrected the spelling of the ``inteval`` attribute to ``interval`` in the :ref:`mjLROpt` struct. - Corrected the spelling of the ``inteval`` attribute to ``interval`` in the :ref:`mjLROpt` struct.
- Mesh texture and normal mappings are now 3-per-triangle rather than 1-per-vertex. Mesh vertices are no longer - Mesh texture and normal mappings are now 3-per-triangle rather than 1-per-vertex. Mesh vertices are no longer
doc/computation.rst
+8 -4
View File
@@ -1422,10 +1422,14 @@ Generation
user full control, however it is a static mechanism (independent of the spatial arrangement of the geoms at runtime) user full control, however it is a static mechanism (independent of the spatial arrangement of the geoms at runtime)
and can be tedious for large models. It is normally used to supplement the output of the "dynamic" mechanism. Dynamic and can be tedious for large models. It is normally used to supplement the output of the "dynamic" mechanism. Dynamic
generation works with bodies rather than geoms; when a body pair is included this means that all geoms attached to generation works with bodies rather than geoms; when a body pair is included this means that all geoms attached to
one body can collide with all geoms attached to the other body. The body pairs are generated via broad-phase one body can collide with all geoms attached to the other body.
collision detection based on a modified sweep-and-prune algorithm. The modification is that the axis for sorting is
chosen as the principal eigenvector of the covariance matrix of all geom centers - which maximizes the spread. If The body pairs are generated via broad-phase collision detection based on a modified sweep-and-prune algorithm. The
broad-phase collision detection is disabled by the user, all body pairs are included in this step. modification is that the axis for sorting is chosen as the principal eigenvector of the covariance matrix of all geom
centers - which maximizes the spread. Then, for each body pair, a mid-phase collision detection using a static
bounding volume hierarchy (a BVH binary tree) of axis-aligned bounding boxes (AABB) is performed. Each body is
equipped with an AABB tree of its geoms, aligned with the body inertial or geom frames for all inner or leaf nodes,
respectively.
Finally, the user can explicitly exclude certain body pairs using the :ref:`exclude <contact-exclude>` element Finally, the user can explicitly exclude certain body pairs using the :ref:`exclude <contact-exclude>` element
in MJCF. Exclusion is applied when "dynamic" or "all" are selected, but not when "pair" is selected. At the end of in MJCF. Exclusion is applied when "dynamic" or "all" are selected, but not when "pair" is selected. At the end of
doc/images/computation/midphase.gif
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 911 KiB

doc/includes/references.h
+17 -1
View File
@@ -221,6 +221,9 @@ struct mjData_ {
mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) mjtNum* qLDiagInv; // 1/diag(D) (nv x 1)
mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1) mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1)
// computed by mj_collisionTree
mjtByte* bvh_active; // volume has been added to collisions (nbvh x 1)
//-------------------------------- POSITION, VELOCITY dependent //-------------------------------- POSITION, VELOCITY dependent
// computed by mj_fwdVelocity // computed by mj_fwdVelocity
@@ -337,8 +340,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags
mjDSBL_ACTUATION = 1<<10, // apply actuation forces mjDSBL_ACTUATION = 1<<10, // apply actuation forces
mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep
mjDSBL_SENSOR = 1<<12, // sensors mjDSBL_SENSOR = 1<<12, // sensors
mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering
mjNDISABLE = 13 // number of disable flags mjNDISABLE = 14 // number of disable flags
} mjtDisableBit; } mjtDisableBit;
typedef enum mjtEnableBit_ { // enable optional feature bitflags typedef enum mjtEnableBit_ { // enable optional feature bitflags
mjENBL_OVERRIDE = 1<<0, // override contact parameters mjENBL_OVERRIDE = 1<<0, // override contact parameters
@@ -649,6 +653,7 @@ struct mjVisual_ { // visualization options
float realtime; // initial real-time factor (1: real time) float realtime; // initial real-time factor (1: real time)
int offwidth; // width of offscreen buffer int offwidth; // width of offscreen buffer
int offheight; // height of offscreen buffer int offheight; // height of offscreen buffer
int treedepth; // depth of the bounding volume hierarchy
} global; } global;
struct { // rendering quality struct { // rendering quality
@@ -744,6 +749,7 @@ struct mjModel_ {
int nu; // number of actuators/controls = dim(ctrl) int nu; // number of actuators/controls = dim(ctrl)
int na; // number of activation states = dim(act) int na; // number of activation states = dim(act)
int nbody; // number of bodies int nbody; // number of bodies
int nbvh; // number of total bounding volumes in all bodies
int njnt; // number of joints int njnt; // number of joints
int ngeom; // number of geoms int ngeom; // number of geoms
int nsite; // number of sites int nsite; // number of sites
@@ -846,6 +852,14 @@ struct mjModel_ {
mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1) mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1)
mjtNum* body_user; // user data (nbody x nuser_body) mjtNum* body_user; // user data (nbody x nuser_body)
int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1) int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1)
int* body_bvhadr; // address of bvh root (nbody x 1)
int* body_bvhnum; // number of bounding volumes (nbody x 1)
// bounding volume hierarchy
int* bvh_depth; // depth in the bounding volume hierarchy (nbvh x 1)
int* bvh_child; // left and right children in tree (nbvh x 2)
int* bvh_geomid; // geom id of the node (non-leaf: -1) (nbvh x 1)
mjtNum* bvh_aabb; // bounding box of node (center, size) (nbvh x 6)
// joints // joints
int* jnt_type; // type of joint (mjtJoint) (njnt x 1) int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
@@ -892,6 +906,7 @@ struct mjModel_ {
mjtNum* geom_solref; // constraint solver reference: contact (ngeom x mjNREF) mjtNum* geom_solref; // constraint solver reference: contact (ngeom x mjNREF)
mjtNum* geom_solimp; // constraint solver impedance: contact (ngeom x mjNIMP) mjtNum* geom_solimp; // constraint solver impedance: contact (ngeom x mjNIMP)
mjtNum* geom_size; // geom-specific size parameters (ngeom x 3) mjtNum* geom_size; // geom-specific size parameters (ngeom x 3)
mjtNum* geom_aabb; // bounding box, (center, size) (ngeom x 6)
mjtNum* geom_rbound; // radius of bounding sphere (ngeom x 1) mjtNum* geom_rbound; // radius of bounding sphere (ngeom x 1)
mjtNum* geom_pos; // local position offset rel. to body (ngeom x 3) mjtNum* geom_pos; // local position offset rel. to body (ngeom x 3)
mjtNum* geom_quat; // local orientation offset rel. to body (ngeom x 4) mjtNum* geom_quat; // local orientation offset rel. to body (ngeom x 4)
@@ -1618,6 +1633,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization
mjVIS_SELECT, // selection point mjVIS_SELECT, // selection point
mjVIS_STATIC, // static bodies mjVIS_STATIC, // static bodies
mjVIS_SKIN, // skin mjVIS_SKIN, // skin
mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy
mjNVISFLAG // number of visualization flags mjNVISFLAG // number of visualization flags
} mjtVisFlag; } mjtVisFlag;
include/mujoco/mjdata.h
+3
View File
@@ -246,6 +246,9 @@ struct mjData_ {
mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) mjtNum* qLDiagInv; // 1/diag(D) (nv x 1)
mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1) mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1)
// computed by mj_collisionTree
mjtByte* bvh_active; // volume has been added to collisions (nbvh x 1)
//-------------------------------- POSITION, VELOCITY dependent //-------------------------------- POSITION, VELOCITY dependent
// computed by mj_fwdVelocity // computed by mj_fwdVelocity
include/mujoco/mjmodel.h
+13 -1
View File
@@ -56,8 +56,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags
mjDSBL_ACTUATION = 1<<10, // apply actuation forces mjDSBL_ACTUATION = 1<<10, // apply actuation forces
mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep
mjDSBL_SENSOR = 1<<12, // sensors mjDSBL_SENSOR = 1<<12, // sensors
mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering
mjNDISABLE = 13 // number of disable flags mjNDISABLE = 14 // number of disable flags
} mjtDisableBit; } mjtDisableBit;
@@ -430,6 +431,7 @@ struct mjVisual_ { // visualization options
float realtime; // initial real-time factor (1: real time) float realtime; // initial real-time factor (1: real time)
int offwidth; // width of offscreen buffer int offwidth; // width of offscreen buffer
int offheight; // height of offscreen buffer int offheight; // height of offscreen buffer
int treedepth; // depth of the bounding volume hierarchy
} global; } global;
struct { // rendering quality struct { // rendering quality
@@ -533,6 +535,7 @@ struct mjModel_ {
int nu; // number of actuators/controls = dim(ctrl) int nu; // number of actuators/controls = dim(ctrl)
int na; // number of activation states = dim(act) int na; // number of activation states = dim(act)
int nbody; // number of bodies int nbody; // number of bodies
int nbvh; // number of total bounding volumes in all bodies
int njnt; // number of joints int njnt; // number of joints
int ngeom; // number of geoms int ngeom; // number of geoms
int nsite; // number of sites int nsite; // number of sites
@@ -635,6 +638,14 @@ struct mjModel_ {
mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1) mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1)
mjtNum* body_user; // user data (nbody x nuser_body) mjtNum* body_user; // user data (nbody x nuser_body)
int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1) int* body_plugin; // plugin instance id (-1 if not in use) (nbody x 1)
int* body_bvhadr; // address of bvh root (nbody x 1)
int* body_bvhnum; // number of bounding volumes (nbody x 1)
// bounding volume hierarchy
int* bvh_depth; // depth in the bounding volume hierarchy (nbvh x 1)
int* bvh_child; // left and right children in tree (nbvh x 2)
int* bvh_geomid; // geom id of the node (non-leaf: -1) (nbvh x 1)
mjtNum* bvh_aabb; // bounding box of node (center, size) (nbvh x 6)
// joints // joints
int* jnt_type; // type of joint (mjtJoint) (njnt x 1) int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
@@ -681,6 +692,7 @@ struct mjModel_ {
mjtNum* geom_solref; // constraint solver reference: contact (ngeom x mjNREF) mjtNum* geom_solref; // constraint solver reference: contact (ngeom x mjNREF)
mjtNum* geom_solimp; // constraint solver impedance: contact (ngeom x mjNIMP) mjtNum* geom_solimp; // constraint solver impedance: contact (ngeom x mjNIMP)
mjtNum* geom_size; // geom-specific size parameters (ngeom x 3) mjtNum* geom_size; // geom-specific size parameters (ngeom x 3)
mjtNum* geom_aabb; // bounding box, (center, size) (ngeom x 6)
mjtNum* geom_rbound; // radius of bounding sphere (ngeom x 1) mjtNum* geom_rbound; // radius of bounding sphere (ngeom x 1)
mjtNum* geom_pos; // local position offset rel. to body (ngeom x 3) mjtNum* geom_pos; // local position offset rel. to body (ngeom x 3)
mjtNum* geom_quat; // local orientation offset rel. to body (ngeom x 4) mjtNum* geom_quat; // local orientation offset rel. to body (ngeom x 4)
include/mujoco/mjvisualize.h
+1
View File
@@ -120,6 +120,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization
mjVIS_SELECT, // selection point mjVIS_SELECT, // selection point
mjVIS_STATIC, // static bodies mjVIS_STATIC, // static bodies
mjVIS_SKIN, // skin mjVIS_SKIN, // skin
mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy
mjNVISFLAG // number of visualization flags mjNVISFLAG // number of visualization flags
} mjtVisFlag; } mjtVisFlag;
include/mujoco/mjxmacro.h
+9
View File
@@ -67,6 +67,7 @@
X( nu ) \ X( nu ) \
X( na ) \ X( na ) \
X( nbody ) \ X( nbody ) \
X( nbvh ) \
X( njnt ) \ X( njnt ) \
X( ngeom ) \ X( ngeom ) \
X( nsite ) \ X( nsite ) \
@@ -176,6 +177,12 @@
X( mjtNum, body_gravcomp, nbody, 1 ) \ X( mjtNum, body_gravcomp, nbody, 1 ) \
X( mjtNum, body_user, nbody, MJ_M(nuser_body) ) \ X( mjtNum, body_user, nbody, MJ_M(nuser_body) ) \
X( int, body_plugin, nbody, 1 ) \ X( int, body_plugin, nbody, 1 ) \
X( int, body_bvhadr, nbody, 1 ) \
X( int, body_bvhnum, nbody, 1 ) \
X( int, bvh_depth, nbvh, 1 ) \
X( int, bvh_child, nbvh, 2 ) \
X( int, bvh_geomid, nbvh, 1 ) \
X( mjtNum, bvh_aabb, nbvh, 6 ) \
X( int, jnt_type, njnt, 1 ) \ X( int, jnt_type, njnt, 1 ) \
X( int, jnt_qposadr, njnt, 1 ) \ X( int, jnt_qposadr, njnt, 1 ) \
X( int, jnt_dofadr, njnt, 1 ) \ X( int, jnt_dofadr, njnt, 1 ) \
@@ -216,6 +223,7 @@
X( mjtNum, geom_solref, ngeom, mjNREF ) \ X( mjtNum, geom_solref, ngeom, mjNREF ) \
X( mjtNum, geom_solimp, ngeom, mjNIMP ) \ X( mjtNum, geom_solimp, ngeom, mjNIMP ) \
X( mjtNum, geom_size, ngeom, 3 ) \ X( mjtNum, geom_size, ngeom, 3 ) \
X( mjtNum, geom_aabb, ngeom, 6 ) \
X( mjtNum, geom_rbound, ngeom, 1 ) \ X( mjtNum, geom_rbound, ngeom, 1 ) \
X( mjtNum, geom_pos, ngeom, 3 ) \ X( mjtNum, geom_pos, ngeom, 3 ) \
X( mjtNum, geom_quat, ngeom, 4 ) \ X( mjtNum, geom_quat, ngeom, 4 ) \
@@ -502,6 +510,7 @@
X( mjtNum, qLD, nM, 1 ) \ X( mjtNum, qLD, nM, 1 ) \
X( mjtNum, qLDiagInv, nv, 1 ) \ X( mjtNum, qLDiagInv, nv, 1 ) \
X( mjtNum, qLDiagSqrtInv, nv, 1 ) \ X( mjtNum, qLDiagSqrtInv, nv, 1 ) \
X( mjtByte, bvh_active, nbvh, 1 ) \
X( mjtNum, ten_velocity, ntendon, 1 ) \ X( mjtNum, ten_velocity, ntendon, 1 ) \
X( mjtNum, actuator_velocity, nu, 1 ) \ X( mjtNum, actuator_velocity, nu, 1 ) \
X( mjtNum, cvel, nbody, 6 ) \ X( mjtNum, cvel, nbody, 6 ) \
introspect/enums.py
+4 -2
View File
@@ -40,7 +40,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjDSBL_ACTUATION', 1024), ('mjDSBL_ACTUATION', 1024),
('mjDSBL_REFSAFE', 2048), ('mjDSBL_REFSAFE', 2048),
('mjDSBL_SENSOR', 4096), ('mjDSBL_SENSOR', 4096),
('mjNDISABLE', 13), ('mjDSBL_MIDPHASE', 8192),
('mjNDISABLE', 14),
]), ]),
)), )),
('mjtEnableBit', ('mjtEnableBit',
@@ -521,7 +522,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjVIS_SELECT', 20), ('mjVIS_SELECT', 20),
('mjVIS_STATIC', 21), ('mjVIS_STATIC', 21),
('mjVIS_SKIN', 22), ('mjVIS_SKIN', 22),
('mjNVISFLAG', 23), ('mjVIS_MIDPHASE', 23),
('mjNVISFLAG', 24),
]), ]),
)), )),
('mjtRndFlag', ('mjtRndFlag',
python/mujoco/structs.cc
+1
View File
@@ -1307,6 +1307,7 @@ PYBIND11_MODULE(_structs, m) {
X(realtime); X(realtime);
X(offwidth); X(offwidth);
X(offheight); X(offheight);
X(treedepth);
#undef X #undef X
py::class_<raw::MjVisualQuality> mjVisualQuality(mjVisual, "Quality"); py::class_<raw::MjVisualQuality> mjVisualQuality(mjVisual, "Quality");
simulate/simulate.cc
+6
View File
@@ -710,6 +710,12 @@ void makerendering(mj::Simulate* sim, int oldstate) {
defFlag[0].pdata = sim->vopt.flags + i; defFlag[0].pdata = sim->vopt.flags + i;
mjui_add(&sim->ui0, defFlag); mjui_add(&sim->ui0, defFlag);
} }
// create tree slider
sim->m->vis.global.treedepth = 0;
mjuiDef defTree[] = {{mjITEM_SLIDERINT, "Tree depth", 2, &sim->m->vis.global.treedepth, "-1 15"}};
mjui_add(&sim->ui0, defTree);
mjui_add(&sim->ui0, defOpenGL); mjui_add(&sim->ui0, defOpenGL);
for (i=0; i<mjNRNDFLAG; i++) { for (i=0; i<mjNRNDFLAG; i++) {
mju::strcpy_arr(defFlag[0].name, mjRNDSTRING[i][0]); mju::strcpy_arr(defFlag[0].name, mjRNDSTRING[i][0]);
src/engine/engine_collision_driver.c
+327 -55
View File
@@ -19,7 +19,6 @@
#include <mujoco/mjdata.h> #include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mjxmacro.h>
#include "engine/engine_callback.h" #include "engine/engine_callback.h"
#include "engine/engine_collision_convex.h" #include "engine/engine_collision_convex.h"
#include "engine/engine_collision_primitive.h" #include "engine/engine_collision_primitive.h"
@@ -48,8 +47,319 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
//------------------------------------ static functions --------------------------------------------
// plane to geom_center squared distance, g1 is a plane
static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) {
mjtNum* mat1 = d->geom_xmat + 9*g1;
mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum dif[3];
mju_sub3(dif, d->geom_xpos + 3*g2, d->geom_xpos + 3*g1);
return mju_dot3(dif, norm);
}
// squared Euclidean distance between 3D vectors
static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
// bounding-sphere collision
static int mj_collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) {
// neither geom is a plane
if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) {
mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin;
if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) {
return 0;
}
}
// one geom is a plane
if (m->geom_type[g1]==mjGEOM_PLANE && m->geom_rbound[g2]>0
&& plane_geom(m, d, g1, g2) > margin+m->geom_rbound[g2]) {
return 0;
}
if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0
&& plane_geom(m, d, g2, g1) > margin+m->geom_rbound[g1]) {
return 0;
}
return 1;
}
//------------------------------------ binary tree search ------------------------------------------
// checks if the proposed collision pair is already present in pair_geom and calls narrow phase
void mj_collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
int startadr, int pairadr) {
// merged: make sure geom pair is not repeated
if (merged) {
// find matching pair
int found = 0;
for (int k=startadr; k<pairadr; k++) {
if ((m->pair_geom1[k]==g1 && m->pair_geom2[k]==g2) ||
(m->pair_geom1[k]==g2 && m->pair_geom2[k]==g1)) {
found = 1;
break;
}
}
// not found: test
if (!found) {
mj_collideGeoms(m, d, g1, g2, 0, 0);
}
}
// not merged: always test
else {
mj_collideGeoms(m, d, g1, g2, 0, 0);
}
}
// oriented bounding boxes collision (see Gottschalk et al.)
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],
mjtNum product[36], mjtNum offset[12], mjtByte* initialize) {
// get infinite dimensions (planes only)
mjtByte inf1[3] = {aabb1[3] >= mjMAXVAL, aabb1[4] >= mjMAXVAL, aabb1[5] >= mjMAXVAL};
mjtByte inf2[3] = {aabb2[3] >= mjMAXVAL, aabb2[4] >= mjMAXVAL, aabb2[5] >= mjMAXVAL};
// if a bounding box is infinite, there must be a collision
if ((inf1[0] && inf1[1] && inf1[2]) || (inf2[0] && inf2[1] && inf2[2])) {
return 1;
}
const mjtNum* aabb[2] = {aabb1, aabb2};
const mjtNum *xmat[2] = {xmat1, xmat2};
const mjtNum *xpos[2] = {xpos1, xpos2};
mjtNum xcenter[2][3], normal[2][3][3];
mjtNum proj[2], radius[2];
mjtByte infinite[2] = {inf1[0] || inf1[1] || inf1[2], inf2[0] || inf2[1] || inf2[2]};
// compute centers in local coordinates
if (product==NULL) {
for (int i=0; i<2; i++) { // bounding boxes
for (int j=0; j<3; j++) { // axes
mju_rotVecMat(xcenter[i], aabb[i], xmat[i]);
mju_addTo3(xcenter[i], xpos[i]);
}
}
}
// compute normals in global coordinates
for (int i=0; i<2; i++) { // bounding boxes
for (int j=0; j<3; j++) { // faces
for (int k=0; k<3; k++) { // world axes
normal[i][j][k] = xmat[i][3*k+j];
}
}
}
// precompute dot products
if (product && offset && *initialize) {
for (int i=0; i<2; i++) { // bodies
for (int j=0; j<2; j++) { // bodies
for (int k=0; k<3; k++) { // axes
for (int l=0; l<3; l++) { // axes
product[18*i + 9*j + 3*k + l] = mju_dot3(normal[i][l], normal[j][k]);
}
offset[6*i + 3*j + k] = mju_dot3(xpos[i], normal[j][k]);
}
}
}
*initialize = 0;
}
// check intersections
for (int j=0; j<2; j++) { // bounding boxes
if (infinite[1-j]) {
continue; // skip test against an infinite body
}
for (int k=0; k<3; k++) { // face
for (int i=0; i<2; i++) { // bounding boxes
if (product==NULL) {
proj[i] = mju_dot3(xcenter[i], normal[j][k]);
radius[i] = fabs(aabb[i][3]*mju_dot3(normal[i][0], normal[j][k])) +
fabs(aabb[i][4]*mju_dot3(normal[i][1], normal[j][k])) +
fabs(aabb[i][5]*mju_dot3(normal[i][2], normal[j][k]));
} else {
int adr = 18*i + 9*j + 3*k;
proj[i] = aabb[i][0] * product[adr + 0] +
aabb[i][1] * product[adr + 1] +
aabb[i][2] * product[adr + 2] +
offset[6*i + 3*j + k];
radius[i] = fabs(aabb[i][3]*product[adr + 0]) +
fabs(aabb[i][4]*product[adr + 1]) +
fabs(aabb[i][5]*product[adr + 2]);
}
}
if (radius[0]+radius[1] < fabs(proj[1]-proj[0])) {
return 0;
}
}
}
return 1;
}
static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) {
// check that the quotient is an integer
_Static_assert(sizeof(mjCollisionTree*) % sizeof(mjtNum) == 0,
"mjCollisionTree has a different size from mjtNum");
return (mjCollisionTree*)mj_stackAlloc(
d, max_stack * sizeof(mjCollisionTree*) / sizeof(mjtNum));
}
// binary search between two body trees
void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
int merged, int startadr, int pairadr) {
const int bvhadr1 = m->body_bvhadr[b1];
const int bvhadr2 = m->body_bvhadr[b2];
const mjtNum* bvh1 = m->bvh_aabb + 6 * bvhadr1;
const mjtNum* bvh2 = m->bvh_aabb + 6 * bvhadr2;
const int* child1 = m->bvh_child + 2 * bvhadr1;
const int* child2 = m->bvh_child + 2 * bvhadr2;
mjtNum product[36] = {}; // 2 bb x 2 bb x 3 axes (body) x 3 axes (world)
mjtNum offset[12] = {}; // 2 bb x 2 bb x 3 axes (world)
mjtByte initialize = 1;
mjMARKSTACK;
// TODO(b/273737633): Store bvh max depths to make this bound tighter.
const int max_stack = m->body_bvhnum[b1] + m->body_bvhnum[b2];
mjCollisionTree* stack = mj_stackAllocTree(d, max_stack);
int nstack = 1;
stack[0].node1 = stack[0].node2 = 0;
while (nstack) {
// pop from stack
nstack--;
int node1 = stack[nstack].node1;
int node2 = stack[nstack].node2;
mjtByte isleaf1 = (child1[2*node1] == -1) && (child1[2*node1+1] == -1);
mjtByte isleaf2 = (child2[2*node2] == -1) && (child2[2*node2+1] == -1);
int nodeid1 = m->bvh_geomid[bvhadr1 + node1];
int nodeid2 = m->bvh_geomid[bvhadr2 + node2];
// both are leaves
if (isleaf1 && isleaf2 && nodeid1!=-1 && nodeid2!=-1) {
if (mj_collideSphere(m, d, nodeid1, nodeid2, /*margin=*/ 0)) {
if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
NULL, NULL, &initialize)) {
mj_collidePair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
}
}
continue;
}
// if no intersection at intermediate levels, stop
if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
d->xipos + 3*b1, d->ximat + 9*b1,
d->xipos + 3*b2, d->ximat + 9*b2,
product, offset, &initialize)) {
continue;
}
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
// keep traversing the tree
if (!isleaf1 && isleaf2) {
for (int i=0; i<2; i++) {
if (child1[2*node1+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = child1[2*node1+i];
stack[nstack].node2 = node2;
nstack++;
}
}
} else if (isleaf1 && !isleaf2) {
for (int i=0; i<2; i++) {
if (child2[2*node2+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = node1;
stack[nstack].node2 = child2[2*node2+i];
nstack++;
}
}
} else {
// compute surface areas of bounding boxes
mjtNum x1 = bvh1[6*node1+3]-bvh1[6*node1+0];
mjtNum y1 = bvh1[6*node1+4]-bvh1[6*node1+1];
mjtNum z1 = bvh1[6*node1+5]-bvh1[6*node1+2];
mjtNum x2 = bvh2[6*node2+3]-bvh2[6*node2+0];
mjtNum y2 = bvh2[6*node2+4]-bvh2[6*node2+1];
mjtNum z2 = bvh2[6*node2+5]-bvh2[6*node2+2];
mjtNum surface1 = x1*y1 + y1*z1 + z1*x1;
mjtNum surface2 = x2*y2 + y2*z2 + z2*x2;
// traverse the hierarchy whose bounding box has the larger surface area
if (surface1 > surface2) {
for (int i = 0; i < 2; i++) {
if (child1[2 * node1 + i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = child1[2 * node1 + i];
stack[nstack].node2 = node2;
nstack++;
}
}
} else {
for (int i = 0; i < 2; i++) {
if (child2[2 * node2 + i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = node1;
stack[nstack].node2 = child2[2*node2+i];
nstack++;
}
}
}
}
}
mjFREESTACK;
}
//----------------------------- collision detection entry point ------------------------------------ //----------------------------- collision detection entry point ------------------------------------
// compare contact pairs by their geom IDs
quicksortfunc(contactcompare, context, el1, el2) {
const mjModel* m = (const mjModel*) context;
mjContact* con1 = (mjContact*)el1;
mjContact* con2 = (mjContact*)el2;
// reproduce the order contacts without mj_collideTree
// normally sorted by (g1, g2), but in mj_collideGeoms, g1 and g2 are swapped based on geom_type.
// here we undo this swapping for the purpose of sorting - needs to be done for each mjContact
int con1_g1 = con1->geom1;
int con1_g2 = con1->geom2;
if (m->geom_type[con1_g1] > m->geom_type[con1_g2]) {
int tmp = con1_g1;
con1_g1 = con1_g2;
con1_g2 = tmp;
}
int con2_g1 = con2->geom1;
int con2_g2 = con2->geom2;
if (m->geom_type[con2_g1] > m->geom_type[con2_g2]) {
int tmp = con2_g1;
con2_g1 = con2_g2;
con2_g2 = tmp;
}
if (con1_g1 < con2_g1) return -1;
if (con1_g1 > con2_g1) return 1;
if (con1_g2 < con2_g2) return -1;
if (con1_g2 > con2_g2) return 1;
return 0;
}
void mj_collision(const mjModel* m, mjData* d) { void mj_collision(const mjModel* m, mjData* d) {
int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr; int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
int nexclude = m->nexclude, npair = m->npair, nbodypair = ((m->nbody-1)*m->nbody)/2; int nexclude = m->nexclude, npair = m->npair, nbodypair = ((m->nbody-1)*m->nbody)/2;
@@ -59,6 +369,9 @@ void mj_collision(const mjModel* m, mjData* d) {
// reset the size of the contact array // reset the size of the contact array
d->ncon = 0; d->ncon = 0;
// reset the visualization flags
memset(d->bvh_active, 0, m->nbvh);
// return if disabled // return if disabled
if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT) if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT)
|| m->nconmax==0 || m->nbody < 2) { || m->nconmax==0 || m->nbody < 2) {
@@ -122,29 +435,17 @@ void mj_collision(const mjModel* m, mjData* d) {
// test all geom pairs within this body pair // test all geom pairs within this body pair
if (m->body_geomnum[b1] && m->body_geomnum[b2]) { if (m->body_geomnum[b1] && m->body_geomnum[b2]) {
for (g1=m->body_geomadr[b1]; g1<m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) { if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2]>1) {
for (g2=m->body_geomadr[b2]; g2<m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) { int ncon_before = d->ncon;
// merged: make sure geom pair is not repeated mj_collideTree(m, d, b1, b2, merged, startadr, pairadr);
if (merged) { int ncon_after = d->ncon;
// find matching pair void* context = (void*) m;
int found = 0; mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before,
for (int k=startadr; k<pairadr; k++) { sizeof(mjContact), contactcompare, context);
if ((m->pair_geom1[k]==g1 && m->pair_geom2[k]==g2) || } else {
(m->pair_geom1[k]==g2 && m->pair_geom2[k]==g1)) { for (g1=m->body_geomadr[b1]; g1<m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) {
found = 1; for (g2=m->body_geomadr[b2]; g2<m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) {
break; mj_collidePair(m, d, g1, g2, merged, startadr, pairadr);
}
}
// not found: test
if (!found) {
mj_collideGeoms(m, d, g1, g2, 0, 0);
}
}
// not merged: always test
else {
mj_collideGeoms(m, d, g1, g2, 0, 0);
} }
} }
} }
@@ -522,22 +823,6 @@ endbroad:
//----------------------------- narrow-phase collision detection ----------------------------------- //----------------------------- narrow-phase collision detection -----------------------------------
// plane : geom_center distance, assuming g1 is plane
static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) {
mjtNum* mat1 = d->geom_xmat + 9*g1;
mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum dif[3];
mju_sub3(dif, d->geom_xpos + 3*g2, d->geom_xpos + 3*g1);
return mju_dot3(dif, norm);
}
// squared Euclidean distance between 3D vectors
static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
// test two geoms for collision, apply filters, add to contact list // test two geoms for collision, apply filters, add to contact list
// flg_user disables filters and uses usermargin // flg_user disables filters and uses usermargin
@@ -615,21 +900,8 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
} }
// bounding sphere filter // bounding sphere filter
if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) { if (!mj_collideSphere(m, d, g1, g2, margin)) {
mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin; return;
if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) {
return;
}
}
// plane : bounding sphere filter
if (m->geom_type[g1]==mjGEOM_PLANE && m->geom_rbound[g2]>0
&& plane_geom(m, d, g1, g2) > margin+m->geom_rbound[g2]) {
return;
}
if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0
&& plane_geom(m, d, g2, g1) > margin+m->geom_rbound[g1]) {
return;
} }
// call collision detector to generate contacts // call collision detector to generate contacts
src/engine/engine_collision_driver.h
+14
View File
@@ -21,14 +21,28 @@
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#else
#include <stdalign.h>
#endif #endif
struct mjCollisionTree_ {
alignas(mjtNum) int node1;
int node2;
};
typedef struct mjCollisionTree_ mjCollisionTree;
// collision function pointers and max contact pairs // collision function pointers and max contact pairs
MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES]; MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES];
// collision detection entry point // collision detection entry point
MJAPI void mj_collision(const mjModel* m, mjData* d); 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],
mjtNum product[36], mjtNum offset[12], mjtByte* initialize);
// broad phase collistion detection; return list of body pairs for narrow phase // broad phase collistion detection; return list of body pairs for narrow phase
int mj_broadphase(const mjModel* m, mjData* d, int* bodypair, int maxpair); int mj_broadphase(const mjModel* m, mjData* d, int* bodypair, int maxpair);
src/engine/engine_io.c
+5 -3
View File
@@ -144,6 +144,7 @@ void mj_defaultVisual(mjVisual* vis) {
vis->global.offwidth = 640; vis->global.offwidth = 640;
vis->global.offheight = 480; vis->global.offheight = 480;
vis->global.realtime = 1.0; vis->global.realtime = 1.0;
vis->global.treedepth = 1;
// rendering quality // rendering quality
vis->quality.shadowsize = 4096; vis->quality.shadowsize = 4096;
@@ -375,7 +376,7 @@ static int safeAddToBufferSize(intptr_t* offset, int* nbuffer, size_t type_size,
// allocate and initialize mjModel structure // allocate and initialize mjModel structure
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt, 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 ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface, int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface, int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
@@ -402,6 +403,7 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
m->nu = nu; m->nu = nu;
m->na = na; m->na = na;
m->nbody = nbody; m->nbody = nbody;
m->nbvh = nbvh;
m->njnt = njnt; m->njnt = njnt;
m->ngeom = ngeom; m->ngeom = ngeom;
m->nsite = nsite; m->nsite = nsite;
@@ -521,7 +523,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
// allocate new model if needed // allocate new model if needed
if (!dest) { if (!dest) {
dest = mj_makeModel(src->nq, src->nv, src->nu, src->na, src->nbody, src->njnt, 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->ngeom, src->nsite, src->ncam, src->nlight, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph, src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface, src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
@@ -702,7 +704,7 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
info[28], info[29], info[30], info[31], info[32], info[33], info[34], info[28], info[29], info[30], info[31], info[32], info[33], info[34],
info[35], info[36], info[37], info[38], info[39], info[40], info[41], info[35], info[36], info[37], info[38], info[39], info[40], info[41],
info[42], info[43], info[44], info[45], info[46], info[47], info[48], info[42], info[43], info[44], info[45], info[46], info[47], info[48],
info[49], info[50], info[51]); info[49], info[50], info[51], info[52]);
if (!m || m->nbuffer!=info[getnint()-1]) { if (!m || m->nbuffer!=info[getnint()-1]) {
if (fp) { if (fp) {
fclose(fp); fclose(fp);
src/engine/engine_io.h
+1 -1
View File
@@ -47,7 +47,7 @@ void mj_defaultStatistic(mjStatistic* stat);
//------------------------------- mjModel ---------------------------------------------------------- //------------------------------- mjModel ----------------------------------------------------------
// allocate mjModel // allocate mjModel
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt, 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 ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface, int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface, int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
src/engine/engine_support.c
+2 -1
View File
@@ -53,7 +53,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
"Filterparent", "Filterparent",
"Actuation", "Actuation",
"Refsafe", "Refsafe",
"Sensor" "Sensor",
"Midphase"
}; };
src/engine/engine_vis_init.c
+2 -1
View File
@@ -88,7 +88,8 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
{"Center of &Mass", "0", "M"}, {"Center of &Mass", "0", "M"},
{"S&elect Point", "0", "E"}, {"S&elect Point", "0", "E"},
{"Static Bo&dy", "1", "D"}, {"Static Bo&dy", "1", "D"},
{"Skin", "1", ";"} {"Skin", "1", ";"},
{"Body Tree", "0", "`"}
}; };
src/engine/engine_vis_visualize.c
+65
View File
@@ -520,6 +520,71 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
} }
} }
// bounding volume hierarchy
if (vopt->flags[mjVIS_MIDPHASE]) {
int bodyid = 0;
float rgba[] = {1, 0, 0, 1};
for (int i = 0; i < m->nbvh; 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] != m->vis.global.treedepth) {
if (!isleaf || m->bvh_depth[i] > m->vis.global.treedepth) {
continue;
}
}
// find geom number
int geomid = m->bvh_geomid[i];
while (i >= m->body_bvhadr[bodyid] + m->body_bvhnum[bodyid]) {
bodyid++;
if (bodyid >= m->nbody) {
mju_error("nbvh outside body range.");
}
}
// compute transformation
mjtNum *aabb = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
mjtNum x[3];
const mjtNum* xpos = isleaf ? d->geom_xpos + 3 * geomid : d->xipos + 3 * bodyid;
const mjtNum* xmat = isleaf ? d->geom_xmat + 9 * geomid : d->ximat + 9 * bodyid;
mju_rotVecMat(x, aabb, xmat);
mju_addTo3(x, xpos);
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
mjtNum dist[3][3];
for (int j=0; j<3; j++) {
for (int k=0; k<3; k++) {
dist[k][j] = aabb[k+3] * xmat[3*j+k];
}
}
int split[3] = {1, 2, 4};
for (int v=0; v<8; v++) {
mjtNum from[3] = {x[0], x[1], x[2]};
for (int k=0; k<3; k++) {
mju_addToScl3(from, dist[k], v&split[k] ? 1 : -1);
}
mjtNum to[3];
for (int k=0; k<3; k++) {
mju_addScl3(to, from, dist[k], 2);
if (!(v&split[k])) {
START
mjv_makeConnector(thisgeom, mjGEOM_LINE, 2,
from[0], from[1], from[2],
to[0], to[1], to[2]);
f2f(thisgeom->rgba, rgba, 4);
FINISH
}
}
}
}
}
// inertia // inertia
objtype = mjOBJ_BODY; objtype = mjOBJ_BODY;
if (vopt->flags[mjVIS_INERTIA]) { if (vopt->flags[mjVIS_INERTIA]) {
src/user/user_model.cc
+20 -1
View File
@@ -234,6 +234,7 @@ mjCModel::~mjCModel() {
void mjCModel::Clear(void) { void mjCModel::Clear(void) {
// sizes set from list lengths // sizes set from list lengths
nbody = 0; nbody = 0;
nbvh = 0;
njnt = 0; njnt = 0;
ngeom = 0; ngeom = 0;
nsite = 0; nsite = 0;
@@ -935,6 +936,11 @@ void mjCModel::SetSizes(void) {
} }
} }
// nbvh
for (i=0; i<nbody; i++) {
nbvh += bodies[i]->nbvh;
}
// nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph // nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph
for (i=0; i<nmesh; i++) { for (i=0; i<nmesh; i++) {
nmeshvert += meshes[i]->nvert; nmeshvert += meshes[i]->nvert;
@@ -1324,6 +1330,7 @@ void mjCModel::CopyTree(mjModel* m) {
int jntadr = 0; // addresses in global arrays int jntadr = 0; // addresses in global arrays
int dofadr = 0; int dofadr = 0;
int qposadr = 0; int qposadr = 0;
int bvh_adr = 0;
// main loop over bodies // main loop over bodies
for (i=0; i<nbody; i++) { for (i=0; i<nbody; i++) {
@@ -1350,6 +1357,17 @@ void mjCModel::CopyTree(mjModel* m) {
m->body_gravcomp[i] = pb->gravcomp; m->body_gravcomp[i] = pb->gravcomp;
copyvec(m->body_user+nuser_body*i, pb->userdata.data(), nuser_body); copyvec(m->body_user+nuser_body*i, pb->userdata.data(), nuser_body);
// bounding volume hierarchy
m->body_bvhadr[i] = (!pb->geoms.empty() ? bvh_adr : -1);
m->body_bvhnum[i] = pb->nbvh;
if (pb->nbvh) {
memcpy(m->bvh_aabb + 6*bvh_adr, pb->bvh.data(), 6*pb->nbvh*sizeof(mjtNum));
memcpy(m->bvh_child + 2*bvh_adr, pb->child.data(), 2*pb->nbvh*sizeof(int));
memcpy(m->bvh_geomid + bvh_adr, pb->nodeid.data(), pb->nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pb->level.data(), pb->nbvh*sizeof(int));
}
bvh_adr += pb->nbvh;
// count free joints // count free joints
int cntfree = 0; int cntfree = 0;
for (j=0; j<(int)pb->joints.size(); j++) { for (j=0; j<(int)pb->joints.size(); j++) {
@@ -1498,6 +1516,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->geom_group[gid] = pg->group; m->geom_group[gid] = pg->group;
m->geom_priority[gid] = pg->priority; m->geom_priority[gid] = pg->priority;
copyvec(m->geom_size+3*gid, pg->size, 3); copyvec(m->geom_size+3*gid, pg->size, 3);
copyvec(m->geom_aabb+6*gid, pg->aabb, 6);
copyvec(m->geom_pos+3*gid, pg->locpos, 3); copyvec(m->geom_pos+3*gid, pg->locpos, 3);
copyvec(m->geom_quat+4*gid, pg->locquat, 4); copyvec(m->geom_quat+4*gid, pg->locquat, 4);
copyvec(m->geom_friction+3*gid, pg->friction, 3); copyvec(m->geom_friction+3*gid, pg->friction, 3);
@@ -2659,7 +2678,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
} }
// create low-level model // create low-level model
m = mj_makeModel(nq, nv, nu, na, nbody, njnt, ngeom, nsite, ncam, nlight, m = mj_makeModel(nq, nv, nu, na, nbody, nbvh, njnt, ngeom, nsite, ncam, nlight,
nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph,
nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert, nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert,
nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude, nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude,
src/user/user_model.h
+1
View File
@@ -215,6 +215,7 @@ class mjCModel {
int nv; // number of degrees of freedom = dim(qvel) int nv; // number of degrees of freedom = dim(qvel)
int nu; // number of actuators/controls int nu; // number of actuators/controls
int na; // number of activation variables int na; // number of activation variables
int nbvh; // number of boundary volume hierarchies
int nmeshvert; // number of vertices in all meshes int nmeshvert; // number of vertices in all meshes
int nmeshnormal; // number of normals in all meshes int nmeshnormal; // number of normals in all meshes
int nmeshtexcoord; // number of texture coordinates in all meshes int nmeshtexcoord; // number of texture coordinates in all meshes
src/user/user_objects.cc
+223
View File
@@ -37,6 +37,7 @@
#include "engine/engine_util_errmem.h" #include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h" #include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h" #include "engine/engine_util_solve.h"
#include "engine/engine_util_spatial.h"
#include "engine/engine_vfs.h" #include "engine/engine_vfs.h"
#include "user/user_model.h" #include "user/user_model.h"
#include "user/user_util.h" #include "user/user_util.h"
@@ -330,6 +331,7 @@ mjCBody::mjCBody(mjCModel* _model) {
subtreedofs = 0; subtreedofs = 0;
gravcomp = 0; gravcomp = 0;
userdata.clear(); userdata.clear();
nbvh = 0;
// plugin variables // plugin variables
is_plugin = false; is_plugin = false;
@@ -344,6 +346,10 @@ mjCBody::mjCBody(mjCModel* _model) {
sites.clear(); sites.clear();
cameras.clear(); cameras.clear();
lights.clear(); lights.clear();
bvh.clear();
child.clear();
nodeid.clear();
level.clear();
} }
@@ -664,6 +670,154 @@ void mjCBody::MakeInertialExplicit() {
} }
// compute bounding volume hierarchy
int mjCBody::MakeBVH(std::vector<mjCGeom *>& elements, int lev) {
int nelements = elements.size();
mjtNum AABB[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
// inverse transformation
mjtNum qinv[4] = {iquat[0], -iquat[1], -iquat[2], -iquat[3]};
for (int i=0; i<nelements; i++) {
// skip visual objects
if (elements[i]->conaffinity==0 && elements[i]->contype==0) {
continue;
}
// transform aabb representation
mjtNum aabb[6] = {elements[i]->aabb[0] - elements[i]->aabb[3],
elements[i]->aabb[1] - elements[i]->aabb[4],
elements[i]->aabb[2] - elements[i]->aabb[5],
elements[i]->aabb[0] + elements[i]->aabb[3],
elements[i]->aabb[1] + elements[i]->aabb[4],
elements[i]->aabb[2] + elements[i]->aabb[5]};
// update node AABB
for (int v=0; v<8; v++) {
mjtNum vert[3], box[3];
vert[0] = (v&1 ? aabb[3] : aabb[0]);
vert[1] = (v&2 ? aabb[4] : aabb[1]);
vert[2] = (v&4 ? aabb[5] : aabb[2]);
// rotate to the body inertial frame
mju_rotVecQuat(box, vert, elements[i]->quat);
box[0] += elements[i]->pos[0] - ipos[0];
box[1] += elements[i]->pos[1] - ipos[1];
box[2] += elements[i]->pos[2] - ipos[2];
mju_rotVecQuat(vert, box, qinv);
AABB[0] = mjMIN(AABB[0], vert[0]);
AABB[1] = mjMIN(AABB[1], vert[1]);
AABB[2] = mjMIN(AABB[2], vert[2]);
AABB[3] = mjMAX(AABB[3], vert[0]);
AABB[4] = mjMAX(AABB[4], vert[1]);
AABB[5] = mjMAX(AABB[5], vert[2]);
}
}
// store current index
int index = nbvh++;
child.push_back(-1);
child.push_back(-1);
nodeid.push_back(-1);
level.push_back(lev);
// transform representation
mjtNum center[] = {(AABB[3] + AABB[0]) / 2, (AABB[4] + AABB[1]) / 2,
(AABB[5] + AABB[2]) / 2};
mjtNum size[] = {(AABB[3] - AABB[0]) / 2, (AABB[4] - AABB[1]) / 2,
(AABB[5] - AABB[2]) / 2};
// store bounding box of the current node
for (int i=0; i<3; i++) {
bvh.push_back(center[i]);
}
for (int i=0; i<3; i++) {
bvh.push_back(size[i]);
}
// leaf node, return
if (nelements==1) {
for (int i=0; i<2; i++) {
child[2*index+i] = -1;
}
nodeid[index] = elements[0]->id;
return index;
}
// find longest axis for splitting the bounding box
mjtNum edges[3] = { AABB[3]-AABB[0], AABB[4]-AABB[1], AABB[5]-AABB[2] };
int axis = edges[0] > edges[1] ? 0 : 1;
axis = edges[axis] > edges[2] ? axis : 2;
// find median along the axis
std::vector<mjtNum> pos(nelements);
for (int i=0; i<nelements; i++) {
// get position in the body inertial frame
mjtNum vert[3] = {elements[i]->pos[0] - ipos[0],
elements[i]->pos[1] - ipos[1],
elements[i]->pos[2] - ipos[2]};
mjtNum lpos[3] = {};
mju_rotVecQuat(lpos, vert, qinv);
pos[i] = lpos[axis];
}
auto m = pos.size()/2;
std::nth_element(pos.begin(), pos.begin() + m, pos.end());
mjtNum threshold = pos[m];
// split using median
std::vector<mjCGeom *> left;
std::vector<mjCGeom *> right;
int skipped = 0;
for (int i=0; i<nelements; i++) {
// get position in the body inertial frame
mjtNum vert[3] = {elements[i]->pos[0] - ipos[0],
elements[i]->pos[1] - ipos[1],
elements[i]->pos[2] - ipos[2]};
mjtNum lpos[3] = {};
mju_rotVecQuat(lpos, vert, qinv);
// skip visual objects
if (elements[i]->conaffinity==0 && elements[i]->contype==0) {
skipped++;
continue;
}
if (lpos[axis] < threshold) {
left.push_back(elements[i]);
} else if (lpos[axis] > threshold) {
right.push_back(elements[i]);
} else {
if (left.size() < right.size()) left.push_back(elements[i]);
else right.push_back(elements[i]);
}
}
// recursive calls
if (!left.empty()) {
child[2*index+0] = MakeBVH(left, lev+1);
}
if (!right.empty()) {
child[2*index+1] = MakeBVH(right, lev+1);
}
// SHOULD NOT OCCUR
if (left.size()+right.size()+skipped != nelements) {
throw mjCError(this, "some elements were lost, body=%s parent=%d children=%d",
name.c_str(), nelements, left.size()+right.size()+skipped);
}
if (child[2*index+0]==-1 && child[2*index+1]==-1 && !skipped) {
throw mjCError(this, "this should have been a leaf, body=%s nelements=%d",
name.c_str(), nelements);
}
return index;
}
// compiler // compiler
void mjCBody::Compile(void) { void mjCBody::Compile(void) {
unsigned int i; unsigned int i;
@@ -774,6 +928,11 @@ void mjCBody::Compile(void) {
MakeLocal(geoms[i]->locpos, geoms[i]->locquat, geoms[i]->pos, geoms[i]->quat); MakeLocal(geoms[i]->locpos, geoms[i]->locquat, geoms[i]->pos, geoms[i]->quat);
} }
// compute bounding volume hierarchy
if (!geoms.empty()) {
MakeBVH(geoms, 0);
}
// compile all joints, count dofs // compile all joints, count dofs
dofnum = 0; dofnum = 0;
for (i=0; i<joints.size(); i++) { for (i=0; i<joints.size(); i++) {
@@ -1311,6 +1470,67 @@ void mjCGeom::SetFluidCoefs(void) {
} }
// compute bounding box
void mjCGeom::ComputeAABB() {
switch (type) {
case mjGEOM_SPHERE:
aabb[3] = aabb[4] = aabb[5] = size[0];
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
break;
case mjGEOM_CAPSULE:
aabb[3] = aabb[4] = size[0];
aabb[5] = size[0] + size[1];
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
break;
case mjGEOM_CYLINDER:
aabb[3] = aabb[4] = size[0];
aabb[5] = size[1];
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
break;
case mjGEOM_MESH:
mjuu_copyvec(aabb, model->meshes[meshid]->aabb, 6);
break;
case mjGEOM_PLANE:
aabb[0] = aabb[1] = aabb[2] = -mjMAXVAL;
aabb[3] = aabb[4] = mjMAXVAL;
aabb[5] = 0;
break;
case mjGEOM_HFIELD:
aabb[0] = -size[0];
aabb[1] = -size[1];
aabb[2] = -model->hfields[hfieldid]->size[3];
aabb[3] = size[0];
aabb[4] = size[1];
aabb[5] = model->hfields[hfieldid]->size[2];
break;
default:
mjuu_copyvec(aabb+3, size, 3);
mjuu_setvec(aabb, -size[0], -size[1], -size[2]);
break;
}
aabb[0] -= margin;
aabb[1] -= margin;
aabb[2] -= margin;
aabb[3] += margin;
aabb[4] += margin;
aabb[5] += margin;
mjtNum pos[] = {(aabb[3] + aabb[0]) / 2, (aabb[4] + aabb[1]) / 2,
(aabb[5] + aabb[2]) / 2};
mjtNum size[] = {(aabb[3] - aabb[0]) / 2, (aabb[4] - aabb[1]) / 2,
(aabb[5] - aabb[2]) / 2};
mjuu_copyvec(aabb, pos, 3);
mjuu_copyvec(aabb+3, size, 3);
}
// compiler // compiler
void mjCGeom::Compile(void) { void mjCGeom::Compile(void) {
@@ -1444,6 +1664,9 @@ void mjCGeom::Compile(void) {
size[2] = mjMAX(fabs(aabb[2]), fabs(aabb[5])); size[2] = mjMAX(fabs(aabb[2]), fabs(aabb[5]));
} }
// compute aabb
ComputeAABB();
// compute geom mass and inertia // compute geom mass and inertia
if (inferinertia) { if (inferinertia) {
if (mjuu_defined(_mass)) { if (mjuu_defined(_mass)) {
src/user/user_objects.h
+11
View File
@@ -213,6 +213,15 @@ class mjCBody : public mjCBase {
int lastdof; // id of last dof int lastdof; // id of last dof
int subtreedofs; // number of dofs in subtree, including self int subtreedofs; // number of dofs in subtree, including self
int MakeBVH(std::vector<mjCGeom *>&, int lev); // make bounding volume hierarchy
int nbvh;
std::vector<mjtNum> bvh; // bounding volume hierarchy
std::vector<int> child; // children of bvh nodes
std::vector<int> nodeid; // id of the geom contained by the node
std::vector<int> level; // levels of bvh
// objects allocated by Add functions // objects allocated by Add functions
std::vector<mjCBody*> bodies; // child bodies std::vector<mjCBody*> bodies; // child bodies
std::vector<mjCGeom*> geoms; // geoms attached to this body std::vector<mjCGeom*> geoms; // geoms attached to this body
@@ -333,6 +342,7 @@ class mjCGeom : public mjCBase {
mjCGeom(mjCModel* = 0, mjCDef* = 0);// constructor mjCGeom(mjCModel* = 0, mjCDef* = 0);// constructor
void Compile(void); // compiler void Compile(void); // compiler
double GetRBound(void); // compute bounding sphere radius double GetRBound(void); // compute bounding sphere radius
void ComputeAABB(void); // compute axis-aligned bounding box
int matid; // id of geom's material int matid; // id of geom's material
int meshid; // id of geom's mesh (-1: none) int meshid; // id of geom's mesh (-1: none)
@@ -341,6 +351,7 @@ class mjCGeom : public mjCBase {
double inertia[3]; // local diagonal inertia double inertia[3]; // local diagonal inertia
double locpos[3]; // local position double locpos[3]; // local position
double locquat[4]; // local orientation double locquat[4]; // local orientation
double aabb[6]; // half-sizes of axis-aligned bounding box
mjCBody* body; // geom's body mjCBody* body; // geom's body
}; };
src/xml/xml_native_reader.cc
+1
View File
@@ -983,6 +983,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
READDSBL("actuation", mjDSBL_ACTUATION) READDSBL("actuation", mjDSBL_ACTUATION)
READDSBL("refsafe", mjDSBL_REFSAFE) READDSBL("refsafe", mjDSBL_REFSAFE)
READDSBL("sensor", mjDSBL_SENSOR) READDSBL("sensor", mjDSBL_SENSOR)
READDSBL("midphase", mjDSBL_MIDPHASE)
#undef READDSBL #undef READDSBL
#define READENBL(NAME, MASK) \ #define READENBL(NAME, MASK) \
src/xml/xml_native_writer.cc
+1
View File
@@ -800,6 +800,7 @@ void mjXWriter::Option(XMLElement* root) {
WRITEDSBL("actuation", mjDSBL_ACTUATION) WRITEDSBL("actuation", mjDSBL_ACTUATION)
WRITEDSBL("refsafe", mjDSBL_REFSAFE) WRITEDSBL("refsafe", mjDSBL_REFSAFE)
WRITEDSBL("sensor", mjDSBL_SENSOR) WRITEDSBL("sensor", mjDSBL_SENSOR)
WRITEDSBL("midphase", mjDSBL_MIDPHASE)
#undef WRITEDSBL #undef WRITEDSBL
#define WRITEENBL(NAME, MASK) \ #define WRITEENBL(NAME, MASK) \
test/engine/engine_collision_driver_test.cc
+21
View File
@@ -23,6 +23,8 @@
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "test/fixture.h" #include "test/fixture.h"
#include "src/engine/engine_collision_driver.h"
namespace mujoco { namespace mujoco {
namespace { namespace {
@@ -216,5 +218,24 @@ TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) {
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(MjCollisionTest, TestOBB) {
mjtNum bvh1[6] = {-1, -1, -1, 1, 1, 1};
mjtNum bvh2[6] = {-1, -1, -1, 1, 1, 1};
mjtNum pos1[3] = {0, 0, 0};
mjtNum mat1[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
mjtNum pos2[3] = {1.71, 1.71, 0}; // just a little more than 1+sqrt(2)/2
mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
EXPECT_THAT(
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, NULL, NULL, 0), true);
// rotate by 45 degrees
mat2[0] = 1./mju_sqrt(2.); mat2[1] = -1./mju_sqrt(2.);
mat2[3] = 1./mju_sqrt(2.); mat2[4] = 1./mju_sqrt(2.);
EXPECT_THAT(
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, NULL, NULL, 0), false);
}
} // namespace } // namespace
} // namespace mujoco } // namespace mujoco
test/engine/testdata/collision_driver/humanoid.xml
Vendored
+248
View File
@@ -0,0 +1,248 @@
<!-- Copyright 2021 DeepMind Technologies Limited
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<mujoco model="Humanoid">
<option timestep="0.005"/>
<visual>
<map force="0.1" zfar="30"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
</visual>
<statistic center="0 0 0.7"/>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
<texture name="body" type="cube" builtin="flat" mark="cross" width="127" height="1278" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
</asset>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<default class="body">
<!-- geoms -->
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body"/>
<default class="thigh">
<geom size=".06"/>
</default>
<default class="shin">
<geom fromto="0 0 0 0 0 -.3" size=".049"/>
</default>
<default class="foot">
<geom size=".027"/>
<default class="foot1">
<geom fromto="-.07 -.01 0 .14 -.03 0"/>
</default>
<default class="foot2">
<geom fromto="-.07 .01 0 .14 .03 0"/>
</default>
</default>
<default class="arm_upper">
<geom size=".04"/>
</default>
<default class="arm_lower">
<geom size=".031"/>
</default>
<default class="hand">
<geom type="sphere" size=".04"/>
</default>
<!-- joints -->
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="joint_big">
<joint damping="5" stiffness="10"/>
<default class="hip_x">
<joint range="-30 10"/>
</default>
<default class="hip_z">
<joint range="-60 35"/>
</default>
<default class="hip_y">
<joint axis="0 1 0" range="-150 20"/>
</default>
<default class="joint_big_stiff">
<joint stiffness="20"/>
</default>
</default>
<default class="knee">
<joint pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
</default>
<default class="ankle">
<joint range="-50 50"/>
<default class="ankle_y">
<joint pos="0 0 .08" axis="0 1 0" stiffness="6"/>
</default>
<default class="ankle_x">
<joint pos="0 0 .04" stiffness="3"/>
</default>
</default>
<default class="shoulder">
<joint range="-85 60"/>
</default>
<default class="elbow">
<joint range="-100 50" stiffness="0"/>
</default>
</default>
</default>
<worldbody>
<light name="spotlight" mode="targetbodycom" target="torso" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 -6 4" cutoff="30"/>
<body name="torso" pos="0 0 1.282" childclass="body">
<light name="top" pos="0 0 2" mode="trackcom"/>
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="root"/>
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="waist_upper" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<body name="head" pos="0 0 .19">
<geom name="head" type="sphere" size=".09"/>
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="waist_lower" pos="-.01 0 -.26">
<geom name="waist_lower" fromto="0 -.06 0 0 .06 0" size=".06"/>
<joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="joint_big_stiff"/>
<joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="joint_big"/>
<body name="pelvis" pos="0 0 -.165">
<joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="joint_big"/>
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<body name="thigh_right" pos="0 -.1 -.04">
<joint name="hip_x_right" axis="1 0 0" class="hip_x"/>
<joint name="hip_z_right" axis="0 0 1" class="hip_z"/>
<joint name="hip_y_right" class="hip_y"/>
<geom name="thigh_right" fromto="0 0 0 0 .01 -.34" class="thigh"/>
<body name="shin_right" pos="0 .01 -.4">
<joint name="knee_right" class="knee"/>
<geom name="shin_right" class="shin"/>
<body name="foot_right" pos="0 0 -.39">
<joint name="ankle_y_right" class="ankle_y"/>
<joint name="ankle_x_right" class="ankle_x" axis="1 0 .5"/>
<geom name="foot1_right" class="foot1"/>
<geom name="foot2_right" class="foot2"/>
</body>
</body>
</body>
<body name="thigh_left" pos="0 .1 -.04">
<joint name="hip_x_left" axis="-1 0 0" class="hip_x"/>
<joint name="hip_z_left" axis="0 0 -1" class="hip_z"/>
<joint name="hip_y_left" class="hip_y"/>
<geom name="thigh_left" fromto="0 0 0 0 -.01 -.34" class="thigh"/>
<body name="shin_left" pos="0 -.01 -.4">
<joint name="knee_left" class="knee"/>
<geom name="shin_left" fromto="0 0 0 0 0 -.3" class="shin"/>
<body name="foot_left" pos="0 0 -.39">
<joint name="ankle_y_left" class="ankle_y"/>
<joint name="ankle_x_left" class="ankle_x" axis="-1 0 -.5"/>
<geom name="foot1_left" class="foot1"/>
<geom name="foot2_left" class="foot2"/>
</body>
</body>
</body>
</body>
</body>
<body name="upper_arm_right" pos="0 -.17 .06">
<joint name="shoulder1_right" axis="2 1 1" class="shoulder"/>
<joint name="shoulder2_right" axis="0 -1 1" class="shoulder"/>
<geom name="upper_arm_right" fromto="0 0 0 .16 -.16 -.16" class="arm_upper"/>
<body name="lower_arm_right" pos=".18 -.18 -.18">
<joint name="elbow_right" axis="0 -1 1" class="elbow"/>
<geom name="lower_arm_right" fromto=".01 .01 .01 .17 .17 .17" class="arm_lower"/>
<body name="hand_right" pos=".18 .18 .18">
<geom name="hand_right" zaxis="1 1 1" class="hand"/>
</body>
</body>
</body>
<body name="upper_arm_left" pos="0 .17 .06">
<joint name="shoulder1_left" axis="-2 1 -1" class="shoulder"/>
<joint name="shoulder2_left" axis="0 -1 -1" class="shoulder"/>
<geom name="upper_arm_left" fromto="0 0 0 .16 .16 -.16" class="arm_upper"/>
<body name="lower_arm_left" pos=".18 .18 -.18">
<joint name="elbow_left" axis="0 -1 -1" class="elbow"/>
<geom name="lower_arm_left" fromto=".01 -.01 .01 .17 -.17 .17" class="arm_lower"/>
<body name="hand_left" pos=".18 -.18 .18">
<geom name="hand_left" zaxis="1 -1 1" class="hand"/>
</body>
</body>
</body>
</body>
</worldbody>
<contact>
<exclude body1="waist_lower" body2="thigh_right"/>
<exclude body1="waist_lower" body2="thigh_left"/>
</contact>
<tendon>
<fixed name="hamstring_right" limited="true" range="-0.3 2">
<joint joint="hip_y_right" coef=".5"/>
<joint joint="knee_right" coef="-.5"/>
</fixed>
<fixed name="hamstring_left" limited="true" range="-0.3 2">
<joint joint="hip_y_left" coef=".5"/>
<joint joint="knee_left" coef="-.5"/>
</fixed>
</tendon>
<actuator>
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
<motor name="abdomen_z" gear="40" joint="abdomen_z"/>
<motor name="abdomen_x" gear="40" joint="abdomen_x"/>
<motor name="hip_x_right" gear="40" joint="hip_x_right"/>
<motor name="hip_z_right" gear="40" joint="hip_z_right"/>
<motor name="hip_y_right" gear="120" joint="hip_y_right"/>
<motor name="knee_right" gear="80" joint="knee_right"/>
<motor name="ankle_x_right" gear="20" joint="ankle_x_right"/>
<motor name="ankle_y_right" gear="20" joint="ankle_y_right"/>
<motor name="hip_x_left" gear="40" joint="hip_x_left"/>
<motor name="hip_z_left" gear="40" joint="hip_z_left"/>
<motor name="hip_y_left" gear="120" joint="hip_y_left"/>
<motor name="knee_left" gear="80" joint="knee_left"/>
<motor name="ankle_x_left" gear="20" joint="ankle_x_left"/>
<motor name="ankle_y_left" gear="20" joint="ankle_y_left"/>
<motor name="shoulder1_right" gear="20" joint="shoulder1_right"/>
<motor name="shoulder2_right" gear="20" joint="shoulder2_right"/>
<motor name="elbow_right" gear="40" joint="elbow_right"/>
<motor name="shoulder1_left" gear="20" joint="shoulder1_left"/>
<motor name="shoulder2_left" gear="20" joint="shoulder2_left"/>
<motor name="elbow_left" gear="40" joint="elbow_left"/>
</actuator>
<keyframe>
<!--
The values below are split into rows for readibility:
torso position
torso orientation
spinal
right leg
left leg
arms
-->
<key name="squat" qpos="0 0 0.596
0.988015 0 0.154359 0
0 0.4 0
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
-0.25 -0.5 -2.5 -2.65 -0.8 0.56
0 0 0 0 0 0"/>
<key name="stand_on_left_leg" qpos="0 0 1.21948
0.971588 -0.179973 0.135318 -0.0729076
-0.0516 -0.202 0.23
-0.24 -0.007 -0.34 -1.76 -0.466 -0.0415
-0.08 -0.01 -0.37 -0.685 -0.35 -0.09
0.109 -0.067 -0.7 -0.05 0.12 0.16"/>
</keyframe>
</mujoco>
test/engine/testdata/collision_driver/midphase.xml
Vendored
+1016
View File
File diff suppressed because it is too large Load Diff
test/engine/testdata/collision_driver/planks.xml
Vendored
+177
View File
@@ -0,0 +1,177 @@
<mujoco>
<default>
<default class="wall">
<geom type="plane" size=".5 .5 .05"/>
</default>
<default class="plank">
<geom type="box" size=".02 .02 .4" euler="-45 0 0"/>
<default class="joist">
<geom euler="0 90 0" rgba=".5 .5 .7 1"/>
</default>
</default>
</default>
​
<statistic extent="1.5" meansize=".05"/>
​
<visual>
<rgba haze="0.15 0.25 0.35 1"/>
<quality shadowsize="4096"/>
<map stiffness="700" shadowscale="0.5" fogstart="10" fogend="15" zfar="40" haze="0.3"/>
</visual>
​
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="512"/>
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
</asset>
​
<worldbody>
<light directional="true" diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false"/>
<light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
​
<geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0" material="matplane" condim="1"/>
<geom name="+x" class="wall" zaxis="1 0 0" pos="-.5 0 -.25"/>
<geom name="-x" class="wall" zaxis="-1 0 0" pos=".5 0 -.25"/>
<geom name="+y" class="wall" zaxis="0 1 0" pos="0 -.5 -.25"/>
<geom name="-y" class="wall" zaxis="0 -1 0" pos="0 .5 -.25"/>
​
<body pos="-.4 -.4 .4">
<freejoint/>
<geom class="plank" pos="0 .25 0"/>
<geom class="plank" pos=".05 .25 0"/>
<geom class="plank" pos=".10 .25 0"/>
<geom class="plank" pos=".15 .25 0"/>
<geom class="plank" pos=".20 .25 0"/>
<geom class="plank" pos=".25 .25 0"/>
<geom class="plank" pos=".30 .25 0"/>
<geom class="plank" pos=".35 .25 0"/>
<geom class="plank" pos=".40 .25 0"/>
<geom class="plank" pos=".45 .25 0"/>
<geom class="plank" pos=".50 .25 0"/>
<geom class="plank" pos=".55 .25 0"/>
<geom class="plank" pos=".60 .25 0"/>
<geom class="plank" pos=".65 .25 0"/>
<geom class="plank" pos=".70 .25 0"/>
<geom class="plank" pos=".75 .25 0"/>
<geom class="plank" pos=".80 .25 0"/>
<geom class="joist" pos=".40 .09 -.2"/>
<geom class="joist" pos=".40 .49 .2"/>
</body>
​
<body pos="-.4 -.4 .5">
<freejoint/>
<geom class="plank" pos="0 .25 0"/>
<geom class="plank" pos=".05 .25 0"/>
<geom class="plank" pos=".10 .25 0"/>
<geom class="plank" pos=".15 .25 0"/>
<geom class="plank" pos=".20 .25 0"/>
<geom class="plank" pos=".25 .25 0"/>
<geom class="plank" pos=".30 .25 0"/>
<geom class="plank" pos=".35 .25 0"/>
<geom class="plank" pos=".40 .25 0"/>
<geom class="plank" pos=".45 .25 0"/>
<geom class="plank" pos=".50 .25 0"/>
<geom class="plank" pos=".55 .25 0"/>
<geom class="plank" pos=".60 .25 0"/>
<geom class="plank" pos=".65 .25 0"/>
<geom class="plank" pos=".70 .25 0"/>
<geom class="plank" pos=".75 .25 0"/>
<geom class="plank" pos=".80 .25 0"/>
<geom class="joist" pos=".40 .09 -.2"/>
<geom class="joist" pos=".40 .49 .2"/>
</body>
​
<body pos="-.4 -.4 .6">
<freejoint/>
<geom class="plank" pos="0 .25 0"/>
<geom class="plank" pos=".05 .25 0"/>
<geom class="plank" pos=".10 .25 0"/>
<geom class="plank" pos=".15 .25 0"/>
<geom class="plank" pos=".20 .25 0"/>
<geom class="plank" pos=".25 .25 0"/>
<geom class="plank" pos=".30 .25 0"/>
<geom class="plank" pos=".35 .25 0"/>
<geom class="plank" pos=".40 .25 0"/>
<geom class="plank" pos=".45 .25 0"/>
<geom class="plank" pos=".50 .25 0"/>
<geom class="plank" pos=".55 .25 0"/>
<geom class="plank" pos=".60 .25 0"/>
<geom class="plank" pos=".65 .25 0"/>
<geom class="plank" pos=".70 .25 0"/>
<geom class="plank" pos=".75 .25 0"/>
<geom class="plank" pos=".80 .25 0"/>
<geom class="joist" pos=".40 .09 -.2"/>
<geom class="joist" pos=".40 .49 .2"/>
</body>
​
<body pos="-.4 -.4 .7">
<freejoint/>
<geom class="plank" pos="0 .25 0"/>
<geom class="plank" pos=".05 .25 0"/>
<geom class="plank" pos=".10 .25 0"/>
<geom class="plank" pos=".15 .25 0"/>
<geom class="plank" pos=".20 .25 0"/>
<geom class="plank" pos=".25 .25 0"/>
<geom class="plank" pos=".30 .25 0"/>
<geom class="plank" pos=".35 .25 0"/>
<geom class="plank" pos=".40 .25 0"/>
<geom class="plank" pos=".45 .25 0"/>
<geom class="plank" pos=".50 .25 0"/>
<geom class="plank" pos=".55 .25 0"/>
<geom class="plank" pos=".60 .25 0"/>
<geom class="plank" pos=".65 .25 0"/>
<geom class="plank" pos=".70 .25 0"/>
<geom class="plank" pos=".75 .25 0"/>
<geom class="plank" pos=".80 .25 0"/>
<geom class="joist" pos=".40 .09 -.2"/>
<geom class="joist" pos=".40 .49 .2"/>
</body>
​
<body pos="-.4 -.4 .8">
<freejoint/>
<geom class="plank" pos="0 .25 0"/>
<geom class="plank" pos=".05 .25 0"/>
<geom class="plank" pos=".10 .25 0"/>
<geom class="plank" pos=".15 .25 0"/>
<geom class="plank" pos=".20 .25 0"/>
<geom class="plank" pos=".25 .25 0"/>
<geom class="plank" pos=".30 .25 0"/>
<geom class="plank" pos=".35 .25 0"/>
<geom class="plank" pos=".40 .25 0"/>
<geom class="plank" pos=".45 .25 0"/>
<geom class="plank" pos=".50 .25 0"/>
<geom class="plank" pos=".55 .25 0"/>
<geom class="plank" pos=".60 .25 0"/>
<geom class="plank" pos=".65 .25 0"/>
<geom class="plank" pos=".70 .25 0"/>
<geom class="plank" pos=".75 .25 0"/>
<geom class="plank" pos=".80 .25 0"/>
<geom class="joist" pos=".40 .09 -.2"/>
<geom class="joist" pos=".40 .49 .2"/>
</body>
​
<body pos="-.4 -.4 .9">
<freejoint/>
<geom class="plank" pos="0 .25 0"/>
<geom class="plank" pos=".05 .25 0"/>
<geom class="plank" pos=".10 .25 0"/>
<geom class="plank" pos=".15 .25 0"/>
<geom class="plank" pos=".20 .25 0"/>
<geom class="plank" pos=".25 .25 0"/>
<geom class="plank" pos=".30 .25 0"/>
<geom class="plank" pos=".35 .25 0"/>
<geom class="plank" pos=".40 .25 0"/>
<geom class="plank" pos=".45 .25 0"/>
<geom class="plank" pos=".50 .25 0"/>
<geom class="plank" pos=".55 .25 0"/>
<geom class="plank" pos=".60 .25 0"/>
<geom class="plank" pos=".65 .25 0"/>
<geom class="plank" pos=".70 .25 0"/>
<geom class="plank" pos=".75 .25 0"/>
<geom class="plank" pos=".80 .25 0"/>
<geom class="joist" pos=".40 .09 -.2"/>
<geom class="joist" pos=".40 .49 .2"/>
</body>
</worldbody>
</mujoco>
unity/Runtime/Bindings/MjBindings.cs
+15 -3
View File
@@ -146,7 +146,8 @@ public enum mjtDisableBit : int{
mjDSBL_ACTUATION = 1024, mjDSBL_ACTUATION = 1024,
mjDSBL_REFSAFE = 2048, mjDSBL_REFSAFE = 2048,
mjDSBL_SENSOR = 4096, mjDSBL_SENSOR = 4096,
mjNDISABLE = 13, mjDSBL_MIDPHASE = 8192,
mjNDISABLE = 14,
} }
public enum mjtEnableBit : int{ public enum mjtEnableBit : int{
mjENBL_OVERRIDE = 1, mjENBL_OVERRIDE = 1,
@@ -485,7 +486,8 @@ public enum mjtVisFlag : int{
mjVIS_SELECT = 20, mjVIS_SELECT = 20,
mjVIS_STATIC = 21, mjVIS_STATIC = 21,
mjVIS_SKIN = 22, mjVIS_SKIN = 22,
mjNVISFLAG = 23, mjVIS_MIDPHASE = 23,
mjNVISFLAG = 24,
} }
public enum mjtRndFlag : int{ public enum mjtRndFlag : int{
mjRND_SHADOW = 0, mjRND_SHADOW = 0,
@@ -1644,6 +1646,7 @@ public unsafe struct mjData_ {
public double* qLD; public double* qLD;
public double* qLDiagInv; public double* qLDiagInv;
public double* qLDiagSqrtInv; public double* qLDiagSqrtInv;
public byte* bvh_active;
public double* ten_velocity; public double* ten_velocity;
public double* actuator_velocity; public double* actuator_velocity;
public double* cvel; public double* cvel;
@@ -1764,6 +1767,7 @@ public unsafe struct global {
public float realtime; public float realtime;
public int offwidth; public int offwidth;
public int offheight; public int offheight;
public int treedepth;
} }
[StructLayout(LayoutKind.Sequential)] [StructLayout(LayoutKind.Sequential)]
@@ -1872,6 +1876,7 @@ public unsafe struct mjModel_ {
public int nu; public int nu;
public int na; public int na;
public int nbody; public int nbody;
public int nbvh;
public int njnt; public int njnt;
public int ngeom; public int ngeom;
public int nsite; public int nsite;
@@ -1960,6 +1965,12 @@ public unsafe struct mjModel_ {
public double* body_gravcomp; public double* body_gravcomp;
public double* body_user; public double* body_user;
public int* body_plugin; public int* body_plugin;
public int* body_bvhadr;
public int* body_bvhnum;
public int* bvh_depth;
public int* bvh_child;
public int* bvh_geomid;
public double* bvh_aabb;
public int* jnt_type; public int* jnt_type;
public int* jnt_qposadr; public int* jnt_qposadr;
public int* jnt_dofadr; public int* jnt_dofadr;
@@ -2000,6 +2011,7 @@ public unsafe struct mjModel_ {
public double* geom_solref; public double* geom_solref;
public double* geom_solimp; public double* geom_solimp;
public double* geom_size; public double* geom_size;
public double* geom_aabb;
public double* geom_rbound; public double* geom_rbound;
public double* geom_pos; public double* geom_pos;
public double* geom_quat; public double* geom_quat;
@@ -2558,7 +2570,7 @@ public unsafe struct mjvOption_ {
public fixed byte tendongroup[6]; public fixed byte tendongroup[6];
public fixed byte actuatorgroup[6]; public fixed byte actuatorgroup[6];
public fixed byte skingroup[6]; public fixed byte skingroup[6];
public fixed byte flags[23]; public fixed byte flags[24];
} }
[StructLayout(LayoutKind.Sequential)] [StructLayout(LayoutKind.Sequential)]