Move tactile sensor to the engine.
PiperOrigin-RevId: 788863133 Change-Id: I3172ebb7641fa8146469da25cba7db794760e7b1
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
89f4789085
commit
51babec9c5
@@ -2160,6 +2160,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
|
||||
return 4;
|
||||
|
||||
case mjSENS_CONTACT:
|
||||
case mjSENS_TACTILE:
|
||||
case mjSENS_USER:
|
||||
return sensor_dim;
|
||||
|
||||
@@ -2588,6 +2589,20 @@ const char* mj_validateReferences(const mjModel* m) {
|
||||
if (nobj != -1 && (m->sensor_refid[i] < -1 || m->sensor_refid[i] >= nobj)) {
|
||||
return "Invalid model: invalid sensor_refid";
|
||||
}
|
||||
if (sensor_type == mjSENS_TACTILE) {
|
||||
int obj_id = m->sensor_objid[i];
|
||||
int parent_body = m->geom_bodyid[obj_id];
|
||||
int collision_geoms = 0;
|
||||
for (int b = 0; b < m->body_geomnum[parent_body]; ++b) {
|
||||
int geom_id = m->body_geomadr[parent_body]+b;
|
||||
if (m->geom_contype[geom_id] || m->geom_conaffinity[geom_id]) {
|
||||
collision_geoms++;
|
||||
}
|
||||
}
|
||||
if (collision_geoms == 0) {
|
||||
return "Touch sensor requires a body with at least one collision geom";
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int i=0; i < m->nexclude; i++) {
|
||||
int exclude_body1 = (m->exclude_signature[i] & 0xFFFF);
|
||||
|
||||
@@ -21,6 +21,7 @@
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjsan.h> // IWYU pragma: keep
|
||||
#include "engine/engine_callback.h"
|
||||
#include "engine/engine_collision_sdf.h"
|
||||
#include "engine/engine_core_smooth.h"
|
||||
#include "engine/engine_crossplatform.h"
|
||||
#include "engine/engine_io.h"
|
||||
@@ -1082,6 +1083,185 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
|
||||
}
|
||||
break;
|
||||
|
||||
case mjSENS_TACTILE: // tactile
|
||||
{
|
||||
mj_markStack(d);
|
||||
|
||||
// get parent weld id
|
||||
int mesh_id = m->sensor_objid[i];
|
||||
int geom_id = m->sensor_refid[i];
|
||||
int parent_body = m->geom_bodyid[geom_id];
|
||||
int parent_weld = m->body_weldid[parent_body];
|
||||
int nchannel = m->sensor_dim[i] / m->mesh_vertnum[mesh_id];
|
||||
|
||||
// clear sensordata and distance matrix
|
||||
mjtNum* sensordata = d->sensordata + m->sensor_adr[i];
|
||||
mju_zero(sensordata, m->sensor_dim[i]);
|
||||
|
||||
// count contacts and get contact geom ids
|
||||
// TODO: use a more efficient C version of unordered_set
|
||||
int* contact_geom_ids = mj_stackAllocInt(d, d->ncon);
|
||||
int ncontact = 0;
|
||||
for (int k = 0; k < d->ncon; k++) {
|
||||
int body1 = m->body_weldid[m->geom_bodyid[d->contact[k].geom1]];
|
||||
int body2 = m->body_weldid[m->geom_bodyid[d->contact[k].geom2]];
|
||||
if (body1 == parent_weld) {
|
||||
int add = 1;
|
||||
for (int j = 0; j < ncontact; j++) {
|
||||
if (contact_geom_ids[j] == d->contact[k].geom2) {
|
||||
add = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (add) {
|
||||
contact_geom_ids[ncontact] = d->contact[k].geom2;
|
||||
ncontact++;
|
||||
}
|
||||
}
|
||||
if (body2 == parent_weld) {
|
||||
int add = 1;
|
||||
for (int j = 0; j < ncontact; j++) {
|
||||
if (contact_geom_ids[j] == d->contact[k].geom1) {
|
||||
add = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (add) {
|
||||
contact_geom_ids[ncontact] = d->contact[k].geom1;
|
||||
ncontact++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// no contacts, return
|
||||
if (ncontact == 0) {
|
||||
mj_freeStack(d);
|
||||
break;
|
||||
}
|
||||
|
||||
// all of the quadrature points are contact points
|
||||
int ncon = m->mesh_vertnum[mesh_id];
|
||||
|
||||
// get site frame
|
||||
mjtNum* geom_pos = d->geom_xpos + 3*geom_id;
|
||||
mjtNum* geom_mat = d->geom_xmat + 9*geom_id;
|
||||
|
||||
// allocate contact forces and positions
|
||||
mjtNum* forcesT = mj_stackAllocNum(d, ncon*3);
|
||||
mju_zero(forcesT, ncon*3);
|
||||
|
||||
// iterate over colliding geoms
|
||||
for (int g = 0; g < ncontact; g++) {
|
||||
int geom = contact_geom_ids[g];
|
||||
int body = m->geom_bodyid[geom];
|
||||
|
||||
// get sdf plugin of the geoms
|
||||
int sdf_instance[2] = {-1, -1};
|
||||
mjtGeom geomtype[2] = {mjGEOM_SDF, mjGEOM_SPHERE};
|
||||
const mjpPlugin* sdf_ptr[2] = {NULL, NULL};
|
||||
if (m->geom_type[geom] == mjGEOM_SDF) {
|
||||
sdf_instance[0] = m->geom_plugin[geom];
|
||||
sdf_ptr[0] = mjc_getSDF(m, geom);
|
||||
} else if (m->geom_type[geom] == mjGEOM_MESH) {
|
||||
sdf_instance[0] = m->geom_dataid[geom];
|
||||
geomtype[0] = (mjtGeom)m->geom_type[geom];
|
||||
} else {
|
||||
sdf_instance[0] = geom;
|
||||
geomtype[0] = (mjtGeom)m->geom_type[geom];
|
||||
}
|
||||
|
||||
// skip mesh geoms not having an octree
|
||||
if (geomtype[0] == mjGEOM_MESH &&
|
||||
m->mesh_octadr[m->geom_dataid[geom]] == -1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// set SDF parameters
|
||||
mjSDF geom_sdf;
|
||||
geom_sdf.id = &sdf_instance[0];
|
||||
geom_sdf.type = mjSDFTYPE_SINGLE;
|
||||
geom_sdf.plugin = &sdf_ptr[0];
|
||||
geom_sdf.geomtype = &geomtype[0];
|
||||
|
||||
// get forces in mesh coordinates
|
||||
int node = 0;
|
||||
float* mesh_vert = m->mesh_vert + 3*m->mesh_vertadr[mesh_id];
|
||||
float* mesh_normal = m->mesh_normal + 3*m->mesh_normaladr[mesh_id];
|
||||
for (int j = 0; j < ncon; j++) {
|
||||
// position in site frame
|
||||
mjtNum pos[3] = {mesh_vert[3 * j + 0], mesh_vert[3 * j + 1],
|
||||
mesh_vert[3 * j + 2]};
|
||||
|
||||
// position in global frame
|
||||
mjtNum xpos[3];
|
||||
mju_mulMatVec3(xpos, geom_mat, pos);
|
||||
mju_addTo3(xpos, geom_pos);
|
||||
|
||||
// position in other geom frame
|
||||
mjtNum lpos[3];
|
||||
mju_sub3(tmp, xpos, d->geom_xpos + 3*geom);
|
||||
mju_mulMatTVec3(lpos, d->geom_xmat + 9*geom, tmp);
|
||||
|
||||
// SDF plugins are in the original mesh frame
|
||||
if (sdf_ptr[0] != NULL) {
|
||||
mjtNum mesh_mat[9];
|
||||
mju_quat2Mat(mesh_mat, m->mesh_quat + 4 * m->geom_dataid[geom]);
|
||||
mju_mulMatVec3(lpos, mesh_mat, lpos);
|
||||
mju_addTo3(lpos, m->mesh_pos + 3 * m->geom_dataid[geom]);
|
||||
}
|
||||
|
||||
// compute distance
|
||||
mjtNum depth = mju_min(mjc_distance(m, d, &geom_sdf, lpos), 0);
|
||||
if (depth == 0) {
|
||||
node++;
|
||||
continue;
|
||||
}
|
||||
|
||||
// get velocity in global frame
|
||||
mjtNum vel_sensor[6], vel_other[6], vel_rel[3];
|
||||
mju_transformSpatial(
|
||||
vel_sensor, d->cvel + 6 * parent_weld, 0, xpos,
|
||||
d->subtree_com + 3 * m->body_rootid[parent_weld], NULL);
|
||||
mju_transformSpatial(
|
||||
vel_other, d->cvel + 6 * body, 0, d->geom_xpos + 3 * geom,
|
||||
d->subtree_com + 3 * m->body_rootid[body], NULL);
|
||||
mju_sub3(vel_rel, vel_sensor+3, vel_other+3);
|
||||
|
||||
mjtNum normal[3] = {mesh_normal[9 * j + 0], mesh_normal[9 * j + 1],
|
||||
mesh_normal[9 * j + 2]};
|
||||
mjtNum tang1[3] = {mesh_normal[9 * j + 3], mesh_normal[9 * j + 4],
|
||||
mesh_normal[9 * j + 5]};
|
||||
mjtNum tang2[3] = {mesh_normal[9 * j + 6], mesh_normal[9 * j + 7],
|
||||
mesh_normal[9 * j + 8]};
|
||||
|
||||
// get contact force/torque, rotate into node frame
|
||||
mju_rotVecQuat(normal, normal, m->mesh_quat + 4 * mesh_id);
|
||||
mju_rotVecQuat(tang1, tang1, m->mesh_quat + 4 * mesh_id);
|
||||
mju_rotVecQuat(tang2, tang2, m->mesh_quat + 4 * mesh_id);
|
||||
mjtNum force[3];
|
||||
mjtNum kMaxDepth = 0.05;
|
||||
mjtNum pressure = depth / (kMaxDepth - depth);
|
||||
mju_scl3(force, normal, pressure);
|
||||
|
||||
// one row of mat^T * force
|
||||
forcesT[0*ncon + node] = mju_dot3(force, normal);
|
||||
forcesT[1*ncon + node] = mju_abs(mju_dot3(vel_rel, tang1));
|
||||
forcesT[2*ncon + node] = mju_abs(mju_dot3(vel_rel, tang2));
|
||||
node++;
|
||||
}
|
||||
}
|
||||
|
||||
// compute sensor output
|
||||
for (int c = 0; c < nchannel; c++) {
|
||||
if (!mju_isZero(forcesT + c*ncon, ncon)) {
|
||||
mju_addTo(sensordata + c*ncon, forcesT + c*ncon, ncon);
|
||||
}
|
||||
}
|
||||
|
||||
mj_freeStack(d);
|
||||
}
|
||||
break;
|
||||
|
||||
case mjSENS_ACCELEROMETER: // accelerometer
|
||||
// tmp = site acceleration, in site frame
|
||||
mj_objectAcceleration(m, d, mjOBJ_SITE, objid, tmp, 1);
|
||||
|
||||
@@ -844,6 +844,86 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
|
||||
}
|
||||
}
|
||||
|
||||
// tactile sensor
|
||||
category = mjCAT_DECOR;
|
||||
objtype = mjOBJ_UNKNOWN;
|
||||
if (vopt->flags[mjVIS_CONTACTPOINT]) {
|
||||
mj_markStack(d);
|
||||
|
||||
for (int id = 0; id < m->nsensor; id++) {
|
||||
if (m->sensor_type[id] == mjSENS_TACTILE) {
|
||||
// get site id and frame
|
||||
int mesh_id = m->sensor_objid[id];
|
||||
int geom_id = m->sensor_refid[id];
|
||||
mjtNum* geom_pos = d->geom_xpos + 3*geom_id;
|
||||
mjtNum* geom_mat = d->geom_xmat + 9*geom_id;
|
||||
mjtNum geom_quat[4];
|
||||
mju_mat2Quat(geom_quat, geom_mat);
|
||||
|
||||
// get sensor data
|
||||
mjtNum* sensordata = d->sensordata + m->sensor_adr[id];
|
||||
int nchannel = m->sensor_dim[id] / m->mesh_vertnum[mesh_id];
|
||||
|
||||
// get maximum absolute normal force
|
||||
mjtNum maxval = 0;
|
||||
for (int j=0; j < m->mesh_vertnum[mesh_id]; j++) {
|
||||
maxval = mju_max(maxval, mju_abs(sensordata[j]));
|
||||
}
|
||||
|
||||
// if no normal force readings, quick return
|
||||
if (!maxval || m->geom_rbound[geom_id] < mjMINVAL) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// draw geoms
|
||||
float* mesh_vert = m->mesh_vert + 3*m->mesh_vertadr[mesh_id];
|
||||
int* face = m->mesh_face + 3*m->mesh_faceadr[mesh_id];
|
||||
for (int i=0; i < m->mesh_facenum[mesh_id]; i++) {
|
||||
if (scn->ngeom >= scn->maxgeom) {
|
||||
mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom);
|
||||
mj_freeStack(d);
|
||||
return;
|
||||
} else {
|
||||
// triangle in global frame
|
||||
mjtNum pos[3][3];
|
||||
for (int j = 0; j < 3; j++) {
|
||||
mjtNum v[3] = {mesh_vert[3 * face[3 * i + j] + 0],
|
||||
mesh_vert[3 * face[3 * i + j] + 1],
|
||||
mesh_vert[3 * face[3 * i + j] + 2]};
|
||||
mju_mulMatVec3(pos[j], geom_mat, v);
|
||||
mju_addTo3(pos[j], geom_pos);
|
||||
}
|
||||
|
||||
// color
|
||||
float rgba[4] = {0, 0, 0, 1.0};
|
||||
mjtNum nval[3] = {0, 0, 0};
|
||||
for (int r = 0; r < mjMIN(nchannel, 3); r++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
mjtNum val = sensordata[r*m->mesh_vertnum[mesh_id] + face[3*i+j]];
|
||||
rgba[r] += mju_abs(val) / maxval;
|
||||
if (val) {
|
||||
nval[r] += 1;
|
||||
}
|
||||
}
|
||||
if (nval[r]) {
|
||||
rgba[r] /= nval[r];
|
||||
}
|
||||
}
|
||||
|
||||
// draw triangles, one per side
|
||||
for (int j = 0; j < 2; j++) {
|
||||
START
|
||||
makeTriangle(thisgeom, pos[0], pos[j ? 1 : 2], pos[j ? 2 : 1], rgba);
|
||||
thisgeom->objid = id;
|
||||
FINISH
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
mj_freeStack(d);
|
||||
}
|
||||
|
||||
// inertia
|
||||
objtype = mjOBJ_BODY;
|
||||
if (vopt->flags[mjVIS_INERTIA]) {
|
||||
|
||||
Reference in New Issue
Block a user