mujoco

// 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.

#include "engine/engine_sensor.h"

#include <stddef.h>

#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#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_core_util.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_memory.h"
#include "engine/engine_plugin.h"
#include "engine/engine_ray.h"
#include "engine/engine_sort.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"



//-------------------------------- utility ---------------------------------------------------------


typedef struct {
  mjtNum criterion;  // criterion for partial sort
  int id;            // index in d->contact
  int flip;          // 0: don't flip the normal, 1: flip the normal
} ContactInfo;

// define ContactSelect: find the k smallest elements of a ContactInfo array
static int ContactInfoCompare(const ContactInfo* a, const ContactInfo* b, void* context) {
  if (a->criterion < b->criterion) return -1;
  if (a->criterion > b->criterion) return 1;

  if (a->id < b->id) return -1;
  if (a->id > b->id) return 1;

  return 0;
}
mjPARTIAL_SORT(ContactSelect, ContactInfo, ContactInfoCompare)


// apply cutoff after each stage
static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
  // process sensors matching stage and having positive cutoff
  for (int i=0; i < m->nsensor; i++) {
    if (m->sensor_needstage[i] == stage && m->sensor_cutoff[i] > 0) {
      // skip fromto sensors
      if (m->sensor_type[i] == mjSENS_GEOMFROMTO) {
        continue;
      }

      // get sensor info
      int adr = m->sensor_adr[i];
      int dim = m->sensor_dim[i];
      mjtNum cutoff = m->sensor_cutoff[i];

      // process all dimensions
      for (int j=0; j < dim; j++) {
        // real: apply on both sides
        if (m->sensor_datatype[i] == mjDATATYPE_REAL) {
          d->sensordata[adr+j] = mju_clip(d->sensordata[adr+j], -cutoff, cutoff);
        }

        // positive: apply on positive side only
        else if (m->sensor_datatype[i] == mjDATATYPE_POSITIVE) {
          d->sensordata[adr+j] = mju_min(cutoff, d->sensordata[adr+j]);
        }
      }
    }
  }
}



// get xpos and xmat pointers to an object in mjData
static void get_xpos_xmat(const mjData* d, mjtObj type, int id, int sensor_id,
                          mjtNum **xpos, mjtNum **xmat) {
  switch (type) {
  case mjOBJ_XBODY:
    *xpos = d->xpos + 3*id;
    *xmat = d->xmat + 9*id;
    break;
  case mjOBJ_BODY:
    *xpos = d->xipos + 3*id;
    *xmat = d->ximat + 9*id;
    break;
  case mjOBJ_GEOM:
    *xpos = d->geom_xpos + 3*id;
    *xmat = d->geom_xmat + 9*id;
    break;
  case mjOBJ_SITE:
    *xpos = d->site_xpos + 3*id;
    *xmat = d->site_xmat + 9*id;
    break;
  case mjOBJ_CAMERA:
    *xpos = d->cam_xpos + 3*id;
    *xmat = d->cam_xmat + 9*id;
    break;
  default:
    mjERROR("invalid object type in sensor %d", sensor_id);
  }
}



// get global quaternion of an object in mjData
static void get_xquat(const mjModel* m, const mjData* d, mjtObj type, int id, int sensor_id,
                      mjtNum *quat) {
  switch (type) {
  case mjOBJ_XBODY:
    mju_copy4(quat, d->xquat+4*id);
    break;
  case mjOBJ_BODY:
    mju_mulQuat(quat, d->xquat+4*id, m->body_iquat+4*id);
    break;
  case mjOBJ_GEOM:
    mju_mulQuat(quat, d->xquat+4*m->geom_bodyid[id], m->geom_quat+4*id);
    break;
  case mjOBJ_SITE:
    mju_mulQuat(quat, d->xquat+4*m->site_bodyid[id], m->site_quat+4*id);
    break;
  case mjOBJ_CAMERA:
    mju_mulQuat(quat, d->xquat+4*m->cam_bodyid[id], m->cam_quat+4*id);
    break;
  default:
    mjERROR("invalid object type in sensor %d", sensor_id);
  }
}



static void cam_project(mjtNum sensordata[2], const mjtNum target_xpos[3],
                        const mjtNum cam_xpos[3], const mjtNum cam_xmat[9],
                        const int cam_res[2], mjtNum cam_fovy,
                        const float cam_intrinsic[4], const float cam_sensorsize[2]) {
  mjtNum fx, fy;

  // translation matrix (4x4)
  mjtNum translation[4][4] = {0};
  translation[0][0] = 1;
  translation[1][1] = 1;
  translation[2][2] = 1;
  translation[3][3] = 1;
  translation[0][3] = -cam_xpos[0];
  translation[1][3] = -cam_xpos[1];
  translation[2][3] = -cam_xpos[2];

  // rotation matrix (4x4)
  mjtNum rotation[4][4] = {0};
  rotation[0][0] = 1;
  rotation[1][1] = 1;
  rotation[2][2] = 1;
  rotation[3][3] = 1;
  for (int i=0; i < 3; i++) {
    for (int j=0; j < 3; j++) {
      rotation[i][j] = cam_xmat[j*3+i];
    }
  }

  // focal transformation matrix (3x4)
  if (cam_sensorsize[0] && cam_sensorsize[1]) {
    fx = cam_intrinsic[0] / cam_sensorsize[0] * cam_res[0];
    fy = cam_intrinsic[1] / cam_sensorsize[1] * cam_res[1];
  } else {
    fx = fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * cam_res[1];
  }

  mjtNum focal[3][4] = {0};
  focal[0][0] = -fx;
  focal[1][1] =  fy;
  focal[2][2] = 1.0;

  // image matrix (3x3)
  mjtNum image[3][3] = {0};
  image[0][0] = 1;
  image[1][1] = 1;
  image[2][2] = 1;
  image[0][2] = (mjtNum)cam_res[0] / 2.0;
  image[1][2] = (mjtNum)cam_res[1] / 2.0;

  // projection matrix (3x4): product of all 4 matrices
  mjtNum proj[3][4] = {0};
  for (int i=0; i < 3; i++) {
    for (int j=0; j < 3; j++) {
      for (int k=0; k < 4; k++) {
        for (int l=0; l < 4; l++) {
          for (int n=0; n < 4; n++) {
            proj[i][n] += image[i][j] * focal[j][k] * rotation[k][l] * translation[l][n];
          }
        }
      }
    }
  }

  // projection matrix multiplies homogenous [x, y, z, 1] vectors
  mjtNum pos_hom[4] = {0, 0, 0, 1};
  mju_copy3(pos_hom, target_xpos);

  // project world coordinates into pixel space, see:
  // https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula
  mjtNum pixel_coord_hom[3] = {0};
  for (int i=0; i < 3; i++) {
    for (int j=0; j < 4; j++) {
      pixel_coord_hom[i] += proj[i][j] * pos_hom[j];
    }
  }

  // avoid dividing by tiny numbers
  mjtNum denom = pixel_coord_hom[2];
  if (mju_abs(denom) < mjMINVAL) {
    if (denom < 0) {
      denom = mju_min(denom, -mjMINVAL);
    } else {
      denom = mju_max(denom, mjMINVAL);
    }
  }

  // compute projection
  sensordata[0] = pixel_coord_hom[0] / denom;
  sensordata[1] = pixel_coord_hom[1] / denom;
}



// check if a contact body/geom matches a sensor spec (type, id)
static int checkMatch(const mjModel* m, int body, int geom, mjtObj type, int id) {
  if (type == mjOBJ_UNKNOWN) return 1;
  if (type == mjOBJ_SITE)    return 1;  // already passed site filter test
  if (type == mjOBJ_GEOM)    return id == geom;
  if (type == mjOBJ_BODY)    return id == body;
  if (type == mjOBJ_XBODY) {
    // traverse up the tree from body, return true if we land on id
    while (body > id) {
      body = m->body_parentid[body];
    }
    return body == id;
  }

  return 0;
}

//   0: no match
//   1: match, use contact normal
//  -1: match, flip contact normal
static int matchContact(const mjModel* m, const mjData* d, int conid,
                        mjtObj type1, int id1, mjtObj type2, int id2) {
  // no criterion: quick match
  if (type1 == mjOBJ_UNKNOWN && type2 == mjOBJ_UNKNOWN) {
    return 1;
  }

  // site filter
  if (type1 == mjOBJ_SITE) {
    if (!mju_insideGeom(d->site_xpos + 3 * id1, d->site_xmat + 9 * id1,
                        m->site_size + 3 * id1, m->site_type[id1], d->contact[conid].pos)) {
      return 0;
    }
  }

  // get geom, body ids
  int geom1 = d->contact[conid].geom[0];
  int geom2 = d->contact[conid].geom[1];
  int body1 = geom1 >= 0 ? m->geom_bodyid[geom1] : -1;
  int body2 = geom2 >= 0 ? m->geom_bodyid[geom2] : -1;

  // check match of sensor objects with contact objects
  int match11 = checkMatch(m, body1, geom1, type1, id1);
  int match12 = checkMatch(m, body2, geom2, type1, id1);
  int match21 = checkMatch(m, body1, geom1, type2, id2);
  int match22 = checkMatch(m, body2, geom2, type2, id2);

  // if a sensor object is specified, it must be involved in the contact
  if (!match11 && !match12) return 0;
  if (!match21 && !match22) return 0;

  // determine direction
  if (type1 != mjOBJ_UNKNOWN && type2 != mjOBJ_UNKNOWN) {
    // both obj1 and obj2 specified: direction depends on order
    int order_regular = match11 && match22;
    int order_reverse = match12 && match21;
    if (order_regular && !order_reverse) return 1;
    if (order_reverse && !order_regular) return -1;
    if (order_regular && order_reverse)  return 1;  // ambiguous, return 1
  } else if (type1 != mjOBJ_UNKNOWN) {
    // only obj1 specified: normal points away from obj1
    return match11 ? 1 : -1;
  } else if (type2 != mjOBJ_UNKNOWN) {
    // only obj2 specified: normal points towards obj2
    return match22 ? 1 : -1;
  }

  // should not occur, all conditions are covered above
  return 0;
}



// fill in output data for contact sensor for all fields
//   if flg_flip > 0, normal/tangent rotate 180 about frame[2]
//   force/torque flip-z s.t. force is equal-and-opposite in new contact frame
static void copySensorData(const mjModel* m, const mjData* d,
                           mjtNum* data[mjNCONDATA], int id, int flg_flip, int nfound) {
  // found flag
  if (data[mjCONDATA_FOUND]) *data[mjCONDATA_FOUND] = nfound;

  // contact force and torque
  if (data[mjCONDATA_FORCE] || data[mjCONDATA_TORQUE]) {
    mjtNum forcetorque[6];
    mj_contactForce(m, d, id, forcetorque);
    if (data[mjCONDATA_FORCE]) {
      mju_copy3(data[mjCONDATA_FORCE], forcetorque);
      if (flg_flip) data[mjCONDATA_FORCE][2] *= -1;
    }
    if (data[mjCONDATA_TORQUE]) {
      mju_copy3(data[mjCONDATA_TORQUE], forcetorque+3);
      if (flg_flip) data[mjCONDATA_TORQUE][2] *= -1;
    }
  }

  // contact penetration distance
  if (data[mjCONDATA_DIST]) {
    *data[mjCONDATA_DIST] = d->contact[id].dist;
  }

  // contact position
  if (data[mjCONDATA_POS]) {
    mju_copy3(data[mjCONDATA_POS], d->contact[id].pos);
  }

  // contact normal
  if (data[mjCONDATA_NORMAL]) {
    mju_copy3(data[mjCONDATA_NORMAL], d->contact[id].frame);
    if (flg_flip) mju_scl3(data[mjCONDATA_NORMAL], data[mjCONDATA_NORMAL], -1);
  }

  // contact first tangent
  if (data[mjCONDATA_TANGENT]) {
    mju_copy3(data[mjCONDATA_TANGENT], d->contact[id].frame+3);
    if (flg_flip) mju_scl3(data[mjCONDATA_TANGENT], data[mjCONDATA_TANGENT], -1);
  }
}



// compute total wrench about one point, in the global frame
static void total_wrench(mjtNum force[3], mjtNum torque[3], const mjtNum point[3], int n,
                        const mjtNum *wrench, const mjtNum *pos, const mjtNum *frame) {
  mju_zero3(force);
  mju_zero3(torque);

  for (int j = 0; j < n; ++j) {
    // rotate force, torque from contact frame to global frame
    mjtNum force_j[3], torque_j[3];
    mju_mulMatTVec3(force_j, frame + 9*j, wrench + 6*j);
    mju_mulMatTVec3(torque_j, frame + 9*j, wrench + 6*j + 3);

    // add to total force, torque
    mju_addTo3(force, force_j);
    mju_addTo3(torque, torque_j);

    // add induced moment:  torque += (pos - point) x force
    mjtNum diff[3];
    mju_sub3(diff, pos + 3*j, point);
    mjtNum induced_torque[3];
    mju_cross(induced_torque, diff, force_j);
    mju_addTo3(torque, induced_torque);
  }
}



//-------------------------------- sensor ----------------------------------------------------------

// position-dependent sensors
void mj_sensorPos(const mjModel* m, mjData* d) {
  int ne = d->ne, nf = d->nf, nefc = d->nefc, nsensor = m->nsensor;
  int nusersensor = 0;

  // disabled sensors: return
  if (mjDISABLED(mjDSBL_SENSOR)) {
    return;
  }

  // process sensors matching stage
  for (int i=0; i < nsensor; i++) {
    mjtSensor type = (mjtSensor) m->sensor_type[i];

    // skip sensor plugins -- these are handled after builtin sensor types
    if (type == mjSENS_PLUGIN) {
      continue;
    }

    if (m->sensor_needstage[i] == mjSTAGE_POS) {
      // get sensor info
      int objtype = m->sensor_objtype[i];
      int objid = m->sensor_objid[i];
      int refid = m->sensor_refid[i];
      int reftype = m->sensor_reftype[i];
      int adr = m->sensor_adr[i];

      mjtNum rvec[3], *xpos, *xmat, *xpos_ref, *xmat_ref;

      // process according to type
      switch (type) {
      case mjSENS_MAGNETOMETER:                           // magnetometer
        mju_mulMatTVec(d->sensordata+adr, d->site_xmat+9*objid, m->opt.magnetic, 3, 3);
        break;

      case mjSENS_CAMPROJECTION:                          // camera projection
        cam_project(d->sensordata+adr, d->site_xpos+3*objid, d->cam_xpos+3*refid,
                    d->cam_xmat+9*refid, m->cam_resolution+2*refid, m->cam_fovy[refid],
                    m->cam_intrinsic+4*refid, m->cam_sensorsize+2*refid);
        break;

      case mjSENS_RANGEFINDER:                            // rangefinder
        rvec[0] = d->site_xmat[9*objid+2];
        rvec[1] = d->site_xmat[9*objid+5];
        rvec[2] = d->site_xmat[9*objid+8];
        d->sensordata[adr] = mj_ray(m, d, d->site_xpos+3*objid, rvec, NULL, 1,
                                    m->site_bodyid[objid], NULL);

        break;

      case mjSENS_JOINTPOS:                               // jointpos
        d->sensordata[adr] = d->qpos[m->jnt_qposadr[objid]];
        break;

      case mjSENS_TENDONPOS:                              // tendonpos
        d->sensordata[adr] = d->ten_length[objid];
        break;

      case mjSENS_ACTUATORPOS:                            // actuatorpos
        d->sensordata[adr] = d->actuator_length[objid];
        break;

      case mjSENS_BALLQUAT:                               // ballquat
        mju_copy4(d->sensordata+adr, d->qpos+m->jnt_qposadr[objid]);
        mju_normalize4(d->sensordata+adr);
        break;

      case mjSENS_JOINTLIMITPOS:                          // jointlimitpos
        d->sensordata[adr] = 0;
        for (int j=ne+nf; j < nefc; j++) {
          if (d->efc_type[j] == mjCNSTR_LIMIT_JOINT && d->efc_id[j] == objid) {
            d->sensordata[adr] = d->efc_pos[j] - d->efc_margin[j];
            break;
          }
        }
        break;

      case mjSENS_TENDONLIMITPOS:                         // tendonlimitpos
        d->sensordata[adr] = 0;
        for (int j=ne+nf; j < nefc; j++) {
          if (d->efc_type[j] == mjCNSTR_LIMIT_TENDON && d->efc_id[j] == objid) {
            d->sensordata[adr] = d->efc_pos[j] - d->efc_margin[j];
            break;
          }
        }
        break;

      case mjSENS_FRAMEPOS:                               // framepos
      case mjSENS_FRAMEXAXIS:                             // framexaxis
      case mjSENS_FRAMEYAXIS:                             // frameyaxis
      case mjSENS_FRAMEZAXIS:                             // framezaxis
        // get xpos and xmat pointers for object frame
        get_xpos_xmat(d, objtype, objid, i, &xpos, &xmat);

        // reference frame unspecified: global frame
        if (refid == -1) {
          if (type == mjSENS_FRAMEPOS) {
            mju_copy3(d->sensordata+adr, xpos);
          } else {
            // offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
            int offset = type - mjSENS_FRAMEXAXIS;
            d->sensordata[adr] = xmat[offset];
            d->sensordata[adr+1] = xmat[offset+3];
            d->sensordata[adr+2] = xmat[offset+6];
          }
        }

        // reference frame specified
        else {
          get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref);
          if (type == mjSENS_FRAMEPOS) {
            mju_sub3(rvec, xpos, xpos_ref);
            mju_mulMatTVec3(d->sensordata+adr, xmat_ref, rvec);
          } else {
            // offset = (0 or 1 or 2) for (x or y or z)-axis sensors, respectively
            int offset = type - mjSENS_FRAMEXAXIS;
            mjtNum axis[3] = {xmat[offset], xmat[offset+3], xmat[offset+6]};
            mju_mulMatTVec3(d->sensordata+adr, xmat_ref, axis);
          }
        }
        break;

      case mjSENS_FRAMEQUAT:                              // framequat
      {
        // get global object quaternion
        mjtNum objquat[4];
        get_xquat(m, d, objtype, objid, i, objquat);

        // reference frame unspecified: copy object quaternion
        if (refid == -1) {
          mju_copy4(d->sensordata+adr, objquat);
        } else {
          // reference frame specified, get global reference quaternion
          mjtNum refquat[4];
          get_xquat(m, d, reftype, refid, i, refquat);

          // relative quaternion
          mju_negQuat(refquat, refquat);
          mju_mulQuat(d->sensordata+adr, refquat, objquat);
        }
      }
      break;

      case mjSENS_SUBTREECOM:                             // subtreecom
        mju_copy3(d->sensordata+adr, d->subtree_com+3*objid);
        break;

      case mjSENS_INSIDESITE:                             // insidesite
        get_xpos_xmat(d, objtype, objid, i, &xpos, &xmat);
        d->sensordata[adr] = mju_insideGeom(d->site_xpos + 3*refid,
                                            d->site_xmat + 9*refid,
                                            m->site_size + 3*refid,
                                            m->site_type[refid],
                                            xpos);
        break;

      case mjSENS_GEOMDIST:                               // signed distance between two geoms
      case mjSENS_GEOMNORMAL:                             // normal direction between two geoms
      case mjSENS_GEOMFROMTO:                             // segment between two geoms
        {
          // use cutoff for collision margin
          mjtNum margin = m->sensor_cutoff[i];

          // initialize outputs
          mjtNum dist = margin;    // collision distance
          mjtNum fromto[6] = {0};  // segment between geoms

          // get lists of geoms to collide
          int n1, id1;
          if (objtype == mjOBJ_BODY) {
            n1 = m->body_geomnum[objid];
            id1 = m->body_geomadr[objid];
          } else {
            n1 = 1;
            id1 = objid;
          }
          int n2, id2;
          if (reftype == mjOBJ_BODY) {
            n2 = m->body_geomnum[refid];
            id2 = m->body_geomadr[refid];
          } else {
            n2 = 1;
            id2 = refid;
          }

          // collide all pairs
          for (int geom1=id1; geom1 < id1+n1; geom1++) {
            for (int geom2=id2; geom2 < id2+n2; geom2++) {
              mjtNum fromto_new[6] = {0};
              mjtNum dist_new = mj_geomDistance(m, d, geom1, geom2, margin, fromto_new);
              if (dist_new < dist) {
                dist = dist_new;
                mju_copy(fromto, fromto_new, 6);
              }
            }
          }

          // write sensordata for this sensor and all subsequent sensors with identical signature
          int write_sensor = 1;
          while (write_sensor) {
            // write geom distance
            if (type == mjSENS_GEOMDIST) {
              d->sensordata[adr] = dist;
            }

            // write distance normal
            else if (type == mjSENS_GEOMNORMAL) {
              mjtNum normal[3] = {fromto[3]-fromto[0], fromto[4]-fromto[1], fromto[5]-fromto[2]};
              if (normal[0] || normal[1] || normal[2]) {
                mju_normalize3(normal);
              }
              mju_copy3(d->sensordata + adr, normal);
            }

            // write distance fromto
            else {
              mju_copy(d->sensordata + adr, fromto, 6);
            }

            // if this is the last sensor, break
            if (i+1 == nsensor) {
              break;
            }

            // type of the next sensor
            mjtSensor type_next = m->sensor_type[i+1];

            // check if signature of next sensor matches this sensor
            write_sensor = (type_next == mjSENS_GEOMDIST   ||
                            type_next == mjSENS_GEOMNORMAL ||
                            type_next == mjSENS_GEOMFROMTO)   &&
                           m->sensor_objtype[i+1] == objtype  &&
                           m->sensor_objid[i+1]   == objid    &&
                           m->sensor_reftype[i+1] == reftype  &&
                           m->sensor_refid[i+1]   == refid    &&
                           m->sensor_cutoff[i+1]  == margin;

            // if signature matches, increment external loop variable i
            if (write_sensor) {
              i++;

              // update adr and type, everything else is the same
              adr = m->sensor_adr[i];
              type = type_next;
            }
          }
        }
        break;

      case mjSENS_E_POTENTIAL:                            // potential energy
        mj_energyPos(m, d);
        d->sensordata[adr] = d->energy[0];
        break;

      case mjSENS_E_KINETIC:                              // kinetic energy
        mj_energyVel(m, d);
        d->sensordata[adr] = d->energy[1];
        break;

      case mjSENS_CLOCK:                                  // clock
        d->sensordata[adr] = d->time;
        break;

      case mjSENS_USER:                                   // user
        nusersensor++;
        break;

      default:
        mjERROR("invalid sensor type in POS stage, sensor %d", i);
      }
    }
  }

  // fill in user sensors if detected
  if (nusersensor && mjcb_sensor) {
    mjcb_sensor(m, d, mjSTAGE_POS);
  }

  // compute plugin sensor values
  if (m->nplugin) {
    const int nslot = mjp_pluginCount();
    for (int i=0; i < m->nplugin; i++) {
      const int slot = m->plugin[i];
      const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
      if (!plugin) {
        mjERROR("invalid plugin slot: %d", slot);
      }
      if ((plugin->capabilityflags & mjPLUGIN_SENSOR) &&
          (plugin->needstage == mjSTAGE_POS || plugin->needstage == mjSTAGE_NONE)) {
        if (!plugin->compute) {
          mjERROR("`compute` is a null function pointer for plugin at slot %d", slot);
        }
        plugin->compute(m, d, i, mjPLUGIN_SENSOR);
      }
    }
  }

  // cutoff
  apply_cutoff(m, d, mjSTAGE_POS);
}



// velocity-dependent sensors
void mj_sensorVel(const mjModel* m, mjData* d) {
  int objtype, objid, reftype, refid, adr, nusersensor = 0;
  int ne = d->ne, nf = d->nf, nefc = d->nefc;
  mjtNum xvel[6];

  // disabled sensors: return
  if (mjDISABLED(mjDSBL_SENSOR)) {
    return;
  }

  // process sensors matching stage
  int subtreeVel = 0;
  for (int i=0; i < m->nsensor; i++) {
    // skip sensor plugins -- these are handled after builtin sensor types
    if (m->sensor_type[i] == mjSENS_PLUGIN) {
      continue;
    }

    if (m->sensor_needstage[i] == mjSTAGE_VEL) {
      // get sensor info
      mjtSensor type = m->sensor_type[i];
      objtype = m->sensor_objtype[i];
      objid = m->sensor_objid[i];
      refid = m->sensor_refid[i];
      reftype = m->sensor_reftype[i];
      adr = m->sensor_adr[i];

      // call mj_subtreeVel when first relevant sensor is encountered
      if (subtreeVel == 0 &&
          (type == mjSENS_SUBTREELINVEL ||
           type == mjSENS_SUBTREEANGMOM ||
           type == mjSENS_USER)) {
        // compute subtree_linvel, subtree_angmom
        mj_subtreeVel(m, d);

        // mark computed
        subtreeVel = 1;
      }

      // process according to type
      switch (type) {
      case mjSENS_VELOCIMETER:                            // velocimeter
        // xvel = site velocity, in site frame
        mj_objectVelocity(m, d, mjOBJ_SITE, objid, xvel, 1);

        // assign linear velocity
        mju_copy3(d->sensordata+adr, xvel+3);
        break;

      case mjSENS_GYRO:                                   // gyro
        // xvel = site velocity, in site frame
        mj_objectVelocity(m, d, mjOBJ_SITE, objid, xvel, 1);

        // assign angular velocity
        mju_copy3(d->sensordata+adr, xvel);
        break;

      case mjSENS_JOINTVEL:                               // jointvel
        d->sensordata[adr] = d->qvel[m->jnt_dofadr[objid]];
        break;

      case mjSENS_TENDONVEL:                              // tendonvel
        d->sensordata[adr] = d->ten_velocity[objid];
        break;

      case mjSENS_ACTUATORVEL:                            // actuatorvel
        d->sensordata[adr] = d->actuator_velocity[objid];
        break;

      case mjSENS_BALLANGVEL:                             // ballangvel
        mju_copy3(d->sensordata+adr, d->qvel+m->jnt_dofadr[objid]);
        break;

      case mjSENS_JOINTLIMITVEL:                          // jointlimitvel
        d->sensordata[adr] = 0;
        for (int j=ne+nf; j < nefc; j++) {
          if (d->efc_type[j] == mjCNSTR_LIMIT_JOINT && d->efc_id[j] == objid) {
            d->sensordata[adr] = d->efc_vel[j];
            break;
          }
        }
        break;

      case mjSENS_TENDONLIMITVEL:                         // tendonlimitvel
        d->sensordata[adr] = 0;
        for (int j=ne+nf; j < nefc; j++) {
          if (d->efc_type[j] == mjCNSTR_LIMIT_TENDON && d->efc_id[j] == objid) {
            d->sensordata[adr] = d->efc_vel[j];
            break;
          }
        }
        break;

      case mjSENS_FRAMELINVEL:                            // framelinvel
      case mjSENS_FRAMEANGVEL:                            // frameangvel
        // xvel = 6D object velocity, in global frame
        mj_objectVelocity(m, d, objtype, objid, xvel, 0);

        if (refid > -1) {  // reference frame specified
          mjtNum *xpos, *xmat, *xpos_ref, *xmat_ref, xvel_ref[6], rel_vel[6], cross[3], rvec[3];

          // in global frame: object and reference position, reference orientation and velocity
          get_xpos_xmat(d, objtype, objid, i, &xpos, &xmat);
          get_xpos_xmat(d, reftype, refid, i, &xpos_ref, &xmat_ref);
          mj_objectVelocity(m, d, reftype, refid, xvel_ref, 0);

          // subtract velocities
          mju_sub(rel_vel, xvel, xvel_ref, 6);

          // linear velocity: add correction due to rotating reference frame
          mju_sub3(rvec, xpos, xpos_ref);
          mju_cross(cross, rvec, xvel_ref);
          mju_addTo3(rel_vel+3, cross);

          // project into reference frame
          mju_mulMatTVec3(xvel, xmat_ref, rel_vel);
          mju_mulMatTVec3(xvel+3, xmat_ref, rel_vel+3);
        }

        // copy linear or angular component
        if (m->sensor_type[i] == mjSENS_FRAMELINVEL) {
          mju_copy3(d->sensordata+adr, xvel+3);
        } else {
          mju_copy3(d->sensordata+adr, xvel);
        }
        break;

      case mjSENS_SUBTREELINVEL:                          // subtreelinvel
        mju_copy3(d->sensordata+adr, d->subtree_linvel+3*objid);
        break;

      case mjSENS_SUBTREEANGMOM:                          // subtreeangmom
        mju_copy3(d->sensordata+adr, d->subtree_angmom+3*objid);
        break;

      case mjSENS_USER:                                   // user
        nusersensor++;
        break;

      default:
        mjERROR("invalid type in VEL stage, sensor %d", i);
      }
    }
  }

  // fill in user sensors if detected
  if (nusersensor && mjcb_sensor) {
    mjcb_sensor(m, d, mjSTAGE_VEL);
  }

  // trigger computation of plugins
  if (m->nplugin) {
    const int nslot = mjp_pluginCount();
    for (int i=0; i < m->nplugin; i++) {
      const int slot = m->plugin[i];
      const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
      if (!plugin) {
        mjERROR("invalid plugin slot: %d", slot);
      }
      if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage == mjSTAGE_VEL) {
        if (!plugin->compute) {
          mjERROR("`compute` is null for plugin at slot %d", slot);
        }
        if (subtreeVel == 0) {
          // compute subtree_linvel, subtree_angmom
          // TODO(b/247107630): add a flag to allow plugin to specify whether it actually needs this
          mj_subtreeVel(m, d);

          // mark computed
          subtreeVel = 1;
        }
        plugin->compute(m, d, i, mjPLUGIN_SENSOR);
      }
    }
  }

  // cutoff
  apply_cutoff(m, d, mjSTAGE_VEL);
}



// acceleration/force-dependent sensors
void mj_sensorAcc(const mjModel* m, mjData* d) {
  int rootid, bodyid, objtype, objid, adr, nusersensor = 0;
  int ne = d->ne, nf = d->nf, nefc = d->nefc, nu = m->nu;
  mjtNum tmp[6], conforce[6], conray[3], frc;
  const mjContact* con;

  // disabled sensors: return
  if (mjDISABLED(mjDSBL_SENSOR)) {
    return;
  }

  // process sensors matching stage
  int rnePost = 0;
  for (int i=0; i < m->nsensor; i++) {
    // skip sensor plugins -- these are handled after builtin sensor types
    if (m->sensor_type[i] == mjSENS_PLUGIN) {
      continue;
    }

    if (m->sensor_needstage[i] == mjSTAGE_ACC) {
      // get sensor info
      mjtSensor type =  m->sensor_type[i];
      objtype = m->sensor_objtype[i];
      objid = m->sensor_objid[i];
      adr = m->sensor_adr[i];

      // call mj_rnePostConstraint when first relevant sensor is encountered
      if (rnePost == 0  && (type == mjSENS_ACCELEROMETER ||
                            type == mjSENS_FORCE         ||
                            type == mjSENS_TORQUE        ||
                            type == mjSENS_FRAMELINACC   ||
                            type == mjSENS_FRAMEANGACC   ||
                            type == mjSENS_USER)) {
        // compute cacc, cfrc_int, cfrc_ext
        mj_rnePostConstraint(m, d);

        // mark computed
        rnePost = 1;
      }

      // process according to type
      switch (type) {
      case mjSENS_TOUCH:                                  // touch
        // extract body data
        bodyid = m->site_bodyid[objid];

        // clear result
        d->sensordata[adr] = 0;

        // find contacts in sensor zone, add normal forces
        for (int j=0; j < d->ncon; j++) {
          // contact pointer, contacting bodies  (-1 for flex)
          con = d->contact + j;
          int conbody[2];
          for (int k=0; k < 2; k++) {
            conbody[k] = (con->geom[k] >= 0) ? m->geom_bodyid[con->geom[k]] : -1;
          }

          // select contacts involving sensorized body
          if (con->efc_address >= 0 && (bodyid == conbody[0] || bodyid == conbody[1])) {
            // get contact force:torque in contact frame
            mj_contactForce(m, d, j, conforce);

            // nothing to do if normal is zero
            if (conforce[0] <= 0) {
              continue;
            }

            // convert contact normal force to global frame, normalize
            mju_scl3(conray, con->frame, conforce[0]);
            mju_normalize3(conray);

            // flip ray direction if sensor is on body2
            if (bodyid == conbody[1]) {
              mju_scl3(conray, conray, -1);
            }

            // add if ray-zone intersection (always true when con->pos inside zone)
            if (mju_rayGeom(d->site_xpos+3*objid, d->site_xmat+9*objid,
                            m->site_size+3*objid, con->pos, conray,
                            m->site_type[objid]) >= 0) {
              d->sensordata[adr] += conforce[0];
            }
          }
        }
        break;

      case mjSENS_CONTACT:                                // contact
        {
          // local reduce enum for readability
          enum {
            REDUCE_NONE     = 0,
            REDUCE_MINDIST  = 1,
            REDUCE_MAXFORCE = 2,
            REDUCE_NETFORCE = 3,
          };

          // prepare sizes and indices
          int dataspec = m->sensor_intprm[i*mjNSENS];
          int size = mju_condataSize(dataspec);  // size of each slot
          int dim = m->sensor_dim[i];            // total sensor array dimension
          int num = dim / size;                  // number of slots
          int reftype = m->sensor_reftype[i];
          int refid = m->sensor_refid[i];
          int reduce = m->sensor_intprm[i*mjNSENS+1];

          // clear all outputs, prepare data pointers
          mjtNum* ptr = d->sensordata + adr;
          mju_zero(ptr, dim);
          mjtNum* data[mjNCONDATA] = {NULL};
          for (int j=0; j < mjNCONDATA; j++) {
            if (dataspec & (1 << j)) {
              data[j] = ptr;
              ptr += mjCONDATA_SIZE[j];
            }
          }

          // prepare for matching loop
          int nmatch = 0;
          mj_markStack(d);
          ContactInfo *match = mjSTACKALLOC(d, d->ncon, ContactInfo);

          // find matching contacts
          for (int j=0; j < d->ncon; j++) {
            // check match condition
            int match_j = matchContact(m, d, j, objtype, objid, reftype, refid);
            if (!match_j) {
              continue;
            }

            // save id and flip flag
            match[nmatch].id = j;
            match[nmatch].flip = match_j < 0;

            // save sorting criterion, if required
            if (reduce == REDUCE_MINDIST) {
              match[nmatch].criterion = d->contact[j].dist;
            } else if (reduce == REDUCE_MAXFORCE) {
              mjtNum forcetorque[6];
              mj_contactForce(m, d, j, forcetorque);
              match[nmatch].criterion = -mju_dot3(forcetorque, forcetorque);
            }

            // increment number of matching contacts
            nmatch++;
          }

          // number of slots to be filled
          int nslot = mjMIN(num, nmatch);

          // partial sort to get bottom nslot contacts if sorted reduction
          if (reduce == REDUCE_MINDIST || reduce == REDUCE_MAXFORCE) {
            ContactInfo *heap = mjSTACKALLOC(d, nslot, ContactInfo);
            ContactSelect(match, heap, nmatch, nslot, NULL);
          }

          // netforce reduction
          else if (reduce == REDUCE_NETFORCE) {
            mjtNum *wrench = mjSTACKALLOC(d, nmatch * 6, mjtNum);
            mjtNum *pos = mjSTACKALLOC(d, nmatch * 3, mjtNum);
            mjtNum *frame = mjSTACKALLOC(d, nmatch * 9, mjtNum);

            // precompute wrenches, positions, and frames, maybe flip wrench
            for (int j=0; j < nmatch; j++) {
              int conid = match[j].id;
              mj_contactForce(m, d, conid, wrench + 6*j);
              mju_copy3(pos + 3*j, d->contact[conid].pos);
              mju_copy(frame + 9*j, d->contact[conid].frame, 9);
              if (match[j].flip) {
                mju_scl(wrench + 6*j , wrench + 6*j, -1, 6);
              }
            }

            // compute point: force-weighted centroid of contact positions
            mjtNum point[3] = {0};
            mjtNum total_force = 0;
            for (int j=0; j < nmatch; j++) {
              mjtNum weight = mju_norm3(wrench + 6*j);
              mju_addToScl3(point, pos + 3*j, weight);
              total_force += weight;
            }
            mju_scl3(point, point, 1.0 / mjMAX(total_force, mjMINVAL));

            // compute total wrench about point, in the global frame
            mjtNum force[3], torque[3];
            total_wrench(force, torque, point, nmatch, wrench, pos, frame);

            // write data to slot 0
            if (data[mjCONDATA_FOUND])   *data[mjCONDATA_FOUND] = nmatch;
            if (data[mjCONDATA_FORCE])   mju_copy3(data[mjCONDATA_FORCE], force);
            if (data[mjCONDATA_TORQUE])  mju_copy3(data[mjCONDATA_TORQUE], torque);
            if (data[mjCONDATA_DIST])    *data[mjCONDATA_DIST] = 0;
            if (data[mjCONDATA_POS])     mju_copy3(data[mjCONDATA_POS], point);
            if (data[mjCONDATA_NORMAL])  data[mjCONDATA_NORMAL][0] = 1;
            if (data[mjCONDATA_TANGENT]) data[mjCONDATA_TANGENT][1] = 1;

            // done with this sensor
            mj_freeStack(d);
            break;
          }

          // copy data into slots, increment pointers
          for (int j=0; j < nslot; j++) {
            copySensorData(m, d, data, match[j].id, match[j].flip, nmatch);
            for (int k=0; k < mjNCONDATA; k++) {
              if (data[k]) data[k] += size;
            }
          }

          mj_freeStack(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 / mju_max(kMaxDepth - depth, mjMINVAL);
              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);

        // assign linear acceleration
        mju_copy3(d->sensordata+adr, tmp+3);
        break;

      case mjSENS_FORCE:                                  // force
        // extract body data
        bodyid = m->site_bodyid[objid];
        rootid = m->body_rootid[bodyid];

        // tmp = interaction force between body and parent, in site frame
        mju_transformSpatial(tmp, d->cfrc_int+6*bodyid, 1,
                             d->site_xpos+3*objid, d->subtree_com+3*rootid, d->site_xmat+9*objid);

        // assign force
        mju_copy3(d->sensordata+adr, tmp+3);
        break;

      case mjSENS_TORQUE:                                 // torque
        // extract body data
        bodyid = m->site_bodyid[objid];
        rootid = m->body_rootid[bodyid];

        // tmp = interaction force between body and parent, in site frame
        mju_transformSpatial(tmp, d->cfrc_int+6*bodyid, 1,
                             d->site_xpos+3*objid, d->subtree_com+3*rootid, d->site_xmat+9*objid);

        // assign torque
        mju_copy3(d->sensordata+adr, tmp);
        break;

      case mjSENS_ACTUATORFRC:                            // actuatorfrc
        d->sensordata[adr] = d->actuator_force[objid];
        break;

      case mjSENS_JOINTACTFRC:                            // jointactfrc
        d->sensordata[adr] = d->qfrc_actuator[m->jnt_dofadr[objid]];
        break;

      case mjSENS_TENDONACTFRC:  // tendonactfrc
        frc = 0.0;
        for (int j=0; j < nu; j++) {
          if (m->actuator_trntype[j] == mjTRN_TENDON && m->actuator_trnid[2*j] == objid) {
            frc += d->actuator_force[j];
          }
        }
        d->sensordata[adr] = frc;
        break;

      case mjSENS_JOINTLIMITFRC:                          // jointlimitfrc
        d->sensordata[adr] = 0;
        for (int j=ne+nf; j < nefc; j++) {
          if (d->efc_type[j] == mjCNSTR_LIMIT_JOINT && d->efc_id[j] == objid) {
            d->sensordata[adr] = d->efc_force[j];
            break;
          }
        }
        break;

      case mjSENS_TENDONLIMITFRC:                         // tendonlimitfrc
        d->sensordata[adr] = 0;
        for (int j=ne+nf; j < nefc; j++) {
          if (d->efc_type[j] == mjCNSTR_LIMIT_TENDON && d->efc_id[j] == objid) {
            d->sensordata[adr] = d->efc_force[j];
            break;
          }
        }
        break;

      case mjSENS_FRAMELINACC:                            // framelinacc
      case mjSENS_FRAMEANGACC:                            // frameangacc
        // get 6D object acceleration, in global frame
        mj_objectAcceleration(m, d, objtype, objid, tmp, 0);

        // copy linear or angular component
        if (m->sensor_type[i] == mjSENS_FRAMELINACC) {
          mju_copy3(d->sensordata+adr, tmp+3);
        } else {
          mju_copy3(d->sensordata+adr, tmp);
        }
        break;

      case mjSENS_USER:                                   // user
        nusersensor++;
        break;

      default:
        mjERROR("invalid type in ACC stage, sensor %d", i);
      }
    }
  }

  // fill in user sensors if detected
  if (nusersensor && mjcb_sensor) {
    mjcb_sensor(m, d, mjSTAGE_ACC);
  }

  // trigger computation of plugins
  if (m->nplugin) {
    const int nslot = mjp_pluginCount();
    for (int i=0; i < m->nplugin; i++) {
      const int slot = m->plugin[i];
      const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
      if (!plugin) {
        mjERROR("invalid plugin slot: %d", slot);
      }
      if ((plugin->capabilityflags & mjPLUGIN_SENSOR) && plugin->needstage == mjSTAGE_ACC) {
        if (!plugin->compute) {
          mjERROR("`compute` is null for plugin at slot %d", slot);
        }
        if (rnePost == 0) {
          // compute cacc, cfrc_int, cfrc_ext
          // TODO(b/247107630): add a flag to allow plugin to specify whether it actually needs this
          mj_rnePostConstraint(m, d);

          // mark computed
          rnePost = 1;
        }
        plugin->compute(m, d, i, mjPLUGIN_SENSOR);
      }
    }
  }

  // cutoff
  apply_cutoff(m, d, mjSTAGE_ACC);
}



//-------------------------------- energy ----------------------------------------------------------

// position-dependent energy (potential)
void mj_energyPos(const mjModel* m, mjData* d) {
  int padr;
  mjtNum dif[3], quat[4], stiffness;

  // init potential energy:  -sum_i body(i).mass * mju_dot(body(i).pos, gravity)
  d->energy[0] = 0;
  if (!mjDISABLED(mjDSBL_GRAVITY)) {
    for (int i=1; i < m->nbody; i++) {
      d->energy[0] -= m->body_mass[i] * mju_dot3(m->opt.gravity, d->xipos+3*i);
    }
  }

  // add joint-level springs
  if (!mjDISABLED(mjDSBL_SPRING)) {
    for (int i=0; i < m->njnt; i++) {
      stiffness = m->jnt_stiffness[i];
      padr = m->jnt_qposadr[i];

      switch ((mjtJoint) m->jnt_type[i]) {
      case mjJNT_FREE:
        mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr);
        d->energy[0] += 0.5*stiffness*mju_dot3(dif, dif);

        // continue with rotations
        padr += 3;
        mjFALLTHROUGH;

      case mjJNT_BALL:
        // convert quaternion difference into angular "velocity"
        mju_copy4(quat, d->qpos+padr);
        mju_normalize4(quat);
        mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr);
        d->energy[0] += 0.5*stiffness*mju_dot3(dif, dif);
        break;

      case mjJNT_SLIDE:
      case mjJNT_HINGE:
        d->energy[0] += 0.5*stiffness*
                        (d->qpos[padr] - m->qpos_spring[padr])*
                        (d->qpos[padr] - m->qpos_spring[padr]);
        break;
      }
    }
  }

  // add tendon-level springs
  if (!mjDISABLED(mjDSBL_SPRING)) {
    for (int i=0; i < m->ntendon; i++) {
      stiffness = m->tendon_stiffness[i];
      mjtNum length = d->ten_length[i];
      mjtNum displacement = 0;

      // compute spring displacement
      mjtNum lower = m->tendon_lengthspring[2*i];
      mjtNum upper = m->tendon_lengthspring[2*i+1];
      if (length > upper) {
        displacement = upper - length;
      } else if (length < lower) {
        displacement = lower - length;
      }

      d->energy[0] += 0.5*stiffness*displacement*displacement;
    }
  }

  // add flex-level springs for dim=1
  if (!mjDISABLED(mjDSBL_SPRING)) {
    for (int i=0; i < m->nflex; i++) {
      stiffness = m->flex_edgestiffness[i];
      if (m->flex_rigid[i] || stiffness == 0 || m->flex_dim[i] > 1) {
        continue;
      }

      // process non-rigid edges of this flex
      int flex_edgeadr = m->flex_edgeadr[i];
      int flex_edgenum = m->flex_edgenum[i];
      for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
        if (!m->flexedge_rigid[e]) {
          mjtNum displacement = m->flexedge_length0[e] - d->flexedge_length[e];
          d->energy[0] += 0.5*stiffness*displacement*displacement;
        };
      }
    }
  }
}



// velocity-dependent energy (kinetic)
void mj_energyVel(const mjModel* m, mjData* d) {
  mj_markStack(d);
  mjtNum *vec = mjSTACKALLOC(d, m->nv, mjtNum);

  // kinetic energy:  0.5 * qvel' * M * qvel
  mj_mulM(m, d, vec, d->qvel);
  d->energy[1] = 0.5*mju_dot(vec, d->qvel, m->nv);

  mj_freeStack(d);
}