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

Add mjGEOM_LINEBOX, used in visualization of collision trees.

Browse Source 
PiperOrigin-RevId: 591224734
Change-Id: I09ad8c4b711021ff4dd5548ba5381d25d5a3d7f8
This commit is contained in:
Yuval Tassa
2023-12-15 05:35:34 -08:00
committed by Copybara-Service
parent f2a967348c
commit 38fa756a20
8 changed files with 84 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+2 -1
View File
@@ -459,10 +459,11 @@ typedef enum mjtGeom_ { // type of geometric shape
mjGEOM_ARROW1, // arrow without wedges
mjGEOM_ARROW2, // arrow in both directions
mjGEOM_LINE, // line
mjGEOM_LINEBOX, // box with line edges
mjGEOM_FLEX, // flex
mjGEOM_SKIN, // skin
mjGEOM_LABEL, // text label
mjGEOM_TRIANGLE, // triangle connecting a frame
mjGEOM_TRIANGLE, // triangle
mjGEOM_NONE = 1001 // missing geom type
} mjtGeom;
include/mujoco/mjmodel.h
+2 -1
View File
@@ -109,10 +109,11 @@ typedef enum mjtGeom_ { // type of geometric shape
mjGEOM_ARROW1, // arrow without wedges
mjGEOM_ARROW2, // arrow in both directions
mjGEOM_LINE, // line
mjGEOM_LINEBOX, // box with line edges
mjGEOM_FLEX, // flex
mjGEOM_SKIN, // skin
mjGEOM_LABEL, // text label
mjGEOM_TRIANGLE, // triangle connecting a frame
mjGEOM_TRIANGLE, // triangle
mjGEOM_NONE = 1001 // missing geom type
} mjtGeom;
introspect/enums.py
+5 -4
View File
@@ -90,10 +90,11 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjGEOM_ARROW1', 101),
('mjGEOM_ARROW2', 102),
('mjGEOM_LINE', 103),
('mjGEOM_FLEX', 104),
('mjGEOM_SKIN', 105),
('mjGEOM_LABEL', 106),
('mjGEOM_TRIANGLE', 107),
('mjGEOM_LINEBOX', 104),
('mjGEOM_FLEX', 105),
('mjGEOM_SKIN', 106),
('mjGEOM_LABEL', 107),
('mjGEOM_TRIANGLE', 108),
('mjGEOM_NONE', 1001),
]),
)),
introspect/enums_test.py
+1 -1
View File
@@ -61,7 +61,7 @@ class EnumsTest(absltest.TestCase):
self.assertEqual(enum_decl.values['mjGEOM_ARROW'], 100)
self.assertEqual(enum_decl.values['mjGEOM_ARROW1'], 101)
self.assertEqual(enum_decl.values['mjGEOM_ARROW2'], 102)
self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 107)
self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 108)
# Skip a few...
self.assertEqual(enum_decl.values['mjGEOM_NONE'], 1001)
python/mujoco/bindings_test.py
+1 -1
View File
@@ -840,7 +840,7 @@ Euler integrator, semi-implicit in velocity.
self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW, 100)
self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW1, 101)
self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW2, 102)
self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 107)
self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 108)
self.assertEqual(mujoco.mjtGeom.mjGEOM_NONE, 1001)
def test_enum_from_int(self):
src/engine/engine_vis_visualize.c
+33 -63
View File
@@ -509,56 +509,6 @@ static int bodycategory(const mjModel* m, int bodyid) {
// draw bounding box
static void drawBoundingBox(mjData* d, mjvScene* scn,
const mjtNum aabb[6], const mjtNum xpos[3],
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);
mju_addTo3(x, xpos);
for (int j=0; j < 3; j++) {
for (int k=0; k < 3; k++) {
dist[k][j] = aabb[k+3] * xmat[3*j+k];
}
}
} else {
mju_copy3(x, aabb);
mju_addTo3(x, xpos);
for (int j=0; j < 3; j++) {
mju_zero3(dist[j]);
dist[j][j] = aabb[j+3];
}
}
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_connector(thisgeom, mjGEOM_LINE, 2, from, to);
f2f(thisgeom->rgba, rgba, 4);
FINISH
}
}
}
}
// computes the camera frustum
static void getFrustum(float zver[2], float zhor[2], float znear,
const float K[4], const float sensorsize[2]) {
@@ -683,6 +633,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
// body BVH
category = mjCAT_DECOR;
objtype = mjOBJ_UNKNOWN;
if (vopt->flags[mjVIS_BODYBVH]) {
float rgba[] = {1, 0, 0, 1};
for (int i = 0; i < m->nbvhstatic; i++) {
@@ -708,20 +660,30 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
break;
}
// compute transformation
mjtNum *aabb = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
// get xpos, xmat, size
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;
const mjtNum *size = isleaf ? m->geom_aabb + 6*geomid + 3 : m->bvh_aabb + 6*i + 3;
// offset xpos with aabb center (not always at frame origin)
const mjtNum *center = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
mjtNum pos[3];
mju_rotVecMat(pos, center, xmat);
mju_addTo3(pos, xpos);
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
drawBoundingBox(d, scn, aabb, xpos, xmat, rgba);
START
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
FINISH
}
}
// flex BVH
category = mjCAT_DECOR;
objtype = mjOBJ_UNKNOWN;
if (vopt->flags[mjVIS_FLEXBVH]) {
float rgba[] = {1, 0, 0, 0.1};
for (int f=0; f < m->nflex; f++) {
@@ -741,10 +703,8 @@ 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;
// b/304453879 : add LINEBOX geom for bounding box visualization
START
mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, aabb+3, aabb, NULL, rgba);
FINISH
}
}
@@ -752,6 +712,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
// mesh BVH
category = mjCAT_DECOR;
objtype = mjOBJ_UNKNOWN;
if (vopt->flags[mjVIS_MESHBVH]) {
float rgba[] = {1, 0, 0, 1};
for (int geomid = 0; geomid < m->ngeom; geomid++) {
@@ -770,11 +732,6 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}
// compute transformation
const mjtNum *aabb = m->bvh_aabb + 6*i;
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
if (!d->bvh_active[i]) {
continue;
}
@@ -782,7 +739,20 @@ 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(d, scn, aabb, xpos, xmat, rgba);
// get xpos, xmat, size
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
const mjtNum *size = m->bvh_aabb + 6*i + 3;
// offset xpos with aabb center (not always at geom origin)
const mjtNum *center = m->bvh_aabb + 6*i;
mjtNum pos[3];
mju_rotVecMat(pos, center, xmat);
mju_addTo3(pos, xpos);
START
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
FINISH
}
}
}
src/render/render_gl3.c
+35
View File
@@ -404,6 +404,41 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
}
break;
case mjGEOM_LINEBOX: // box with line edges
glLineWidth(1.5*con->lineWidth);
lighting = glIsEnabled(GL_LIGHTING);
glDisable(GL_LIGHTING);
// bottom face
glBegin(GL_LINE_LOOP);
glVertex3f(-size[0], -size[1], -size[2]);
glVertex3f( size[0], -size[1], -size[2]);
glVertex3f( size[0], size[1], -size[2]);
glVertex3f(-size[0], size[1], -size[2]);
glEnd();
// top face
glBegin(GL_LINE_LOOP);
glVertex3f(-size[0], -size[1], size[2]);
glVertex3f( size[0], -size[1], size[2]);
glVertex3f( size[0], size[1], size[2]);
glVertex3f(-size[0], size[1], size[2]);
glEnd();
// vertical edges
glBegin(GL_LINES);
glVertex3f(-size[0], -size[1], -size[2]);
glVertex3f(-size[0], -size[1], size[2]);
glVertex3f( size[0], -size[1], -size[2]);
glVertex3f( size[0], -size[1], size[2]);
glVertex3f( size[0], size[1], -size[2]);
glVertex3f( size[0], size[1], size[2]);
glVertex3f(-size[0], size[1], -size[2]);
glVertex3f(-size[0], size[1], size[2]);
glEnd();
glLineWidth(con->lineWidth);
if (lighting) {
glEnable(GL_LIGHTING);
}
break;
case mjGEOM_TRIANGLE: // triangle
glBegin(GL_TRIANGLES);
glVertex3f(0, 0, 0);
unity/Runtime/Bindings/MjBindings.cs
+5 -4
View File
@@ -191,10 +191,11 @@ public enum mjtGeom : int{
mjGEOM_ARROW1 = 101,
mjGEOM_ARROW2 = 102,
mjGEOM_LINE = 103,
mjGEOM_FLEX = 104,
mjGEOM_SKIN = 105,
mjGEOM_LABEL = 106,
mjGEOM_TRIANGLE = 107,
mjGEOM_LINEBOX = 104,
mjGEOM_FLEX = 105,
mjGEOM_SKIN = 106,
mjGEOM_LABEL = 107,
mjGEOM_TRIANGLE = 108,
mjGEOM_NONE = 1001,
}
public enum mjtCamLight : int{