Skip to content

Commit

Permalink
Add visual friction cones and pyramids
Browse files Browse the repository at this point in the history
  • Loading branch information
@yuvaltassa
yuvaltassa committed Jun 26, 2024
1 parent 6be4e06 commit 91ca5fe
Show file tree
Hide file tree
Showing 5 changed files with 131 additions and 37 deletions.
3 changes: 3 additions & 0 deletions include/mujoco/mjmodel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,8 @@ typedef enum mjtGeom_ { // type of geometric shape
mjGEOM_SKIN, // skin
mjGEOM_LABEL, // text label
mjGEOM_TRIANGLE, // triangle
mjGEOM_CONE, // cone
mjGEOM_PYRAMID, // pyramid

mjGEOM_NONE = 1001 // missing geom type
} mjtGeom;
Expand Down Expand Up @@ -469,6 +471,7 @@ struct mjVisual_ { // visualization options
int offwidth; // width of offscreen buffer
int offheight; // height of offscreen buffer
int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid)
// int conecontact; // geom for contact visualization (0: cylinder, 1: cone)
int bvactive; // visualize active bounding volumes (0: no, 1: yes)
} global;

Expand Down
71 changes: 51 additions & 20 deletions src/engine/engine_vis_visualize.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include "engine/engine_vis_visualize.h"

#include <math.h>
#include <stddef.h>
#include <string.h>

Expand Down Expand Up @@ -99,33 +100,69 @@ static void islandColor(float rgba[4], int islanddofadr) {
static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
const mjvOption* vopt, mjvScene* scn) {
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;
mjContact* con;
mjvGeom* thisgeom;
mjtByte split;

// fast return if all relevant features are disabled
if (!flags[mjVIS_CONTACTPOINT] && !flags[mjVIS_CONTACTFORCE] && vopt->frame != mjFRAME_CONTACT) {
return;
}

// visual sizes
mjtNum scl = m->stat.meansize;
float conwidth = m->vis.scale.contactwidth * scl;
float conheight = m->vis.scale.contactheight * scl;
float hypot = sqrtf(conwidth*conwidth + conheight*conheight);
mjtNum framelength = m->vis.scale.framelength * scl / 2;
mjtNum framewidth = m->vis.scale.framewidth * scl / 2;

// loop over contacts
for (int i=0; i < d->ncon; i++) {
// get pointer
con = d->contact + i;
mjContact* con = d->contact + i;

// mat = contact rotation matrix (normal along z)
mjtNum tmp[9], mat[9];
mju_copy(tmp, con->frame+3, 6);
mju_copy(tmp+6, con->frame, 3);
mju_transpose(mat, tmp, 3, 3);

// get bodyflex ids
int bf[2];
bf[0] = (con->geom[0] >= 0) ? m->geom_bodyid[con->geom[0]] : m->nbody + con->flex[0];
bf[1] = (con->geom[1] >= 0) ? m->geom_bodyid[con->geom[1]] : m->nbody + con->flex[1];

// contact point
if (flags[mjVIS_CONTACTPOINT]) {
START
thisgeom->type = mjGEOM_CYLINDER;
thisgeom->size[0] = thisgeom->size[1] = m->vis.scale.contactwidth * scl;
thisgeom->size[2] = m->vis.scale.contactheight * scl;

// non-world contact, use cylinder
if (bf[0] > 0 && bf[1] > 0) {
thisgeom->type = mjGEOM_CYLINDER;
thisgeom->size[0] = thisgeom->size[1] = conwidth;
thisgeom->size[2] = conheight;
}

// world contact, use pyramid or cone
else {
// geom type according to cone
thisgeom->type = m->opt.cone == mjCONE_PYRAMIDAL ? mjGEOM_PYRAMID : mjGEOM_CONE;

// get tangential friction coefs, convert tangent to sines
float f0 = con->friction[0];
float f1 = con->friction[1];
float sin0 = sqrtf(f0*f0 / (1 + f0*f0));
float sin1 = sqrtf(f1*f1 / (1 + f1*f1));

// get cosine, hypotenuse
float sinmax = mjMAX(sin0, sin1);
float cos = sqrtf(1 - sinmax*sinmax);

// set sizes
thisgeom->size[0] = hypot * sin0;
thisgeom->size[1] = hypot * sin1;
thisgeom->size[2] = hypot * cos;
}

mju_n2f(thisgeom->pos, con->pos, 3);
mju_n2f(thisgeom->mat, mat, 9);

Expand Down Expand Up @@ -191,18 +228,16 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,

// contact frame
if (vopt->frame == mjFRAME_CONTACT) {
// set length and width of axis cylinders using half regular frame scaling
framelength = m->vis.scale.framelength * scl / 2;
framewidth = m->vis.scale.framewidth * scl / 2;

// draw the three axes (separate geoms)
for (int j=0; j < 3; j++) {
START

// prepare axis
mjtNum axis[3];
for (int k=0; k < 3; k++) {
axis[k] = (j == k ? framelength : 0);
}
mjtNum vec[3];
mju_mulMatVec(vec, mat, axis, 3, 3);

// create a cylinder
Expand Down Expand Up @@ -230,21 +265,24 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
mju_transpose(mat, con->frame, 3, 3);

// get contact force:torque in contact frame
mjtNum confrc[6];
mj_contactForce(m, d, i, confrc);

// contact force
if (flags[mjVIS_CONTACTFORCE]) {
// get force, fill zeros if only normal
mjtNum frc[3];
mju_zero3(frc);
mju_copy(frc, confrc, mjMIN(3, con->dim));
if (mju_norm3(frc) < mjMINVAL) {
continue;
}

// render combined or split
split = (flags[mjVIS_CONTACTSPLIT] && con->dim > 1);
mjtByte split = (flags[mjVIS_CONTACTSPLIT] && con->dim > 1);
for (int j = (split ? 1 : 0); j < (split ? 3 : 1); j++) {
// set vec to combined, normal or friction force, in world frame
mjtNum vec[3];
switch (j) {
case 0: // combined
mju_mulMatVec(vec, mat, frc, 3, 3);
Expand All @@ -264,13 +302,6 @@ 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 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 bodyflex with higher id
if (bf[0] > bf[1]) {
mju_scl3(vec, vec, -1);
Expand Down
80 changes: 64 additions & 16 deletions src/render/render_context.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -755,23 +755,33 @@ static void disk(int sign, int nSlice, int nStack) {


// set vertex and normal on cone, given az, r
static void setVertexCone(float* v, float* n, float az, float r) {
// apex 0: cone open to +z, 1: cone open to -z
static void setVertexCone(float v[3], float n[3], float az, float r, int apex) {
const float scale = 1.0f/sqrtf(2.0f);

// vertex
v[0] = cosf(az) * r;
v[1] = sinf(az) * r;
v[2] = 1 - r;
v[2] = apex ? 1 - r : r;

// normal
n[0] = cosf(az) * scale;
n[1] = sinf(az) * scale;
n[0] = cosf(az) * scale * (apex ? 1 : -1);
n[1] = sinf(az) * scale * (apex ? 1 : -1);
n[2] = scale;
}


// make open cone
static void cone(int nSlice, int nStack) {

static void flip(float vec[3]) {
vec[0] = -vec[0];
vec[1] = -vec[1];
vec[2] = -vec[2];
}



// make open cone, apex at either 0 or 1, one-sided or two sided surface
static void cone(int nSlice, int nStack, int apex, int twosided) {
float az1, az2, r1, r2;
float v1[3], v2[3], v3[3], v4[3];
float n1[3], n2[3], n3[3], n4[3];
Expand All @@ -784,10 +794,10 @@ static void cone(int nSlice, int nStack) {
az2 = (2.0f*mjPI * (j+1)) / (float)nSlice;

// compute triangle vertices
setVertexCone(v1, n1, az1, r1);
setVertexCone(v2, n2, az2, r1);
setVertexCone(v1, n1, az1, r1, apex);
setVertexCone(v2, n2, az2, r1, apex);
v3[0] = v3[1] = 0;
v3[2] = 1;
v3[2] = apex;
n3[0] = n1[0]+n2[0];
n3[1] = n1[1]+n2[1];
n3[2] = n1[2]+n2[2];
Expand All @@ -800,6 +810,19 @@ static void cone(int nSlice, int nStack) {
glVertex3fv(v2);
glNormal3fv(n3);
glVertex3fv(v3);

// flip normals, make reverse triangle
if (twosided) {
flip(n3);
flip(n2);
flip(n1);
glNormal3fv(n3);
glVertex3fv(v3);
glNormal3fv(n2);
glVertex3fv(v2);
glNormal3fv(n1);
glVertex3fv(v1);
}
}
glEnd();

Expand All @@ -814,10 +837,10 @@ static void cone(int nSlice, int nStack) {
az2 = (2.0f*mjPI * (j+1)) / (float)nSlice;

// compute quad vertices
setVertexCone(v1, n1, az1, r2);
setVertexCone(v2, n2, az2, r2);
setVertexCone(v3, n3, az2, r1);
setVertexCone(v4, n4, az1, r1);
setVertexCone(v1, n1, az1, r2, apex);
setVertexCone(v2, n2, az2, r2, apex);
setVertexCone(v3, n3, az2, r1, apex);
setVertexCone(v4, n4, az1, r1, apex);

// make quad
glNormal3fv(n1);
Expand All @@ -828,13 +851,28 @@ static void cone(int nSlice, int nStack) {
glVertex3fv(v3);
glNormal3fv(n4);
glVertex3fv(v4);

// flip normals, make reverse quad
if (twosided) {
flip(n4);
flip(n3);
flip(n2);
flip(n1);
glNormal3fv(n4);
glVertex3fv(v4);
glNormal3fv(n3);
glVertex3fv(v3);
glNormal3fv(n2);
glVertex3fv(v2);
glNormal3fv(n1);
glVertex3fv(v1);
}
}
}
glEnd();
}



// set one vertex and normal on cylinder, given az and h
static void setVertexCylinder(float* v, float* n, float az, float h) {
v[0] = cosf(az);
Expand Down Expand Up @@ -1027,11 +1065,21 @@ static void makeBuiltin(const mjModel* m, mjrContext* con) {
glEnd();
glEndList();

// cone, bottom at z=0
// cone, apex at z=1
glNewList(con->baseBuiltin + mjrCONE, GL_COMPILE);
cone(numslices, numstacks);
cone(numslices, numstacks, 1, 0);
disk(0, numslices, numstacks);
glEndList();

// cone, apex at z=0
glNewList(con->baseBuiltin + mjrOPENCONE, GL_COMPILE);
cone(numslices, numstacks, 0, 1);
glEndList();

// pyramid, apex at z=0
glNewList(con->baseBuiltin + mjrOPENPYRAMID, GL_COMPILE);
cone(4, numstacks, 0, 1);
glEndList();
}


Expand Down
2 changes: 2 additions & 0 deletions src/render/render_context.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ enum {
mjrHAZE,
mjrBOX,
mjrCONE,
mjrOPENCONE,
mjrOPENPYRAMID,
mjrNUM
};

Expand Down
12 changes: 11 additions & 1 deletion src/render/render_gl3.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -447,6 +447,16 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
glEnd();
break;

case mjGEOM_CONE: // open cone
glScalef(size[0], size[1], size[2]);
glCallList(con->baseBuiltin + mjrOPENCONE);
break;

case mjGEOM_PYRAMID: // open pyramid
glScalef(size[0], size[1], size[2]);
glCallList(con->baseBuiltin + mjrOPENPYRAMID);
break;

case mjGEOM_FLEX: // flex
{
// no texture for vertices and edges
Expand Down Expand Up @@ -655,7 +665,7 @@ static void initLights(mjvScene* scn) {

// init light model
glLightModelfv(GL_LIGHT_MODEL_AMBIENT, rgba_global);
glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 0);
glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 1);
glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER, 1);

// set light properties
Expand Down

0 comments on commit 91ca5fe

Please sign in to comment.