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_forward.h"

#include <stddef.h>
#include <stdio.h>

#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjsan.h>  // IWYU pragma: keep
#include <mujoco/mjplugin.h>
#include "engine/engine_callback.h"
#include "engine/engine_collision_driver.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_core_util.h"
#include "engine/engine_derivative.h"
#include "engine/engine_inverse.h"
#include "engine/engine_island.h"
#include "engine/engine_macro.h"
#include "engine/engine_memory.h"
#include "engine/engine_passive.h"
#include "engine/engine_plugin.h"
#include "engine/engine_sensor.h"
#include "engine/engine_sleep.h"
#include "engine/engine_solver.h"
#include "engine/engine_support.h"
#include "engine/engine_inline.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_sparse.h"
#include "engine/engine_thread.h"



//--------------------------- check values ---------------------------------------------------------

// check positions, reset if bad
void mj_checkPos(const mjModel* m, mjData* d) {
  int nq = m->nq;
  const mjtNum* qpos = d->qpos;
  for (int i=0; i < nq; i++) {
    if (mju_isBad(qpos[i])) {
      mj_warning(d, mjWARN_BADQPOS, i);
      if (!mjDISABLED(mjDSBL_AUTORESET)) {
        mj_resetData(m, d);
      }
      d->warning[mjWARN_BADQPOS].number++;
      d->warning[mjWARN_BADQPOS].lastinfo = i;
      return;
    }
  }
}


// check velocities, reset if bad
void mj_checkVel(const mjModel* m, mjData* d) {
  int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
  int nv = sleep_filter ? d->nv_awake : m->nv;

  for (int j=0; j < nv; j++) {
    int i = sleep_filter ? d->dof_awake_ind[j] : j;

    if (mju_isBad(d->qvel[i])) {
      mj_warning(d, mjWARN_BADQVEL, i);
      if (!mjDISABLED(mjDSBL_AUTORESET)) {
        mj_resetData(m, d);
      }
      d->warning[mjWARN_BADQVEL].number++;
      d->warning[mjWARN_BADQVEL].lastinfo = i;
      return;
    }
  }
}


// check accelerations, reset if bad
void mj_checkAcc(const mjModel* m, mjData* d) {
  int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
  int nv = sleep_filter ? d->nv_awake : m->nv;

  for (int j=0; j < nv; j++) {
    int i = sleep_filter ? d->dof_awake_ind[j] : j;

    if (mju_isBad(d->qacc[i])) {
      mj_warning(d, mjWARN_BADQACC, i);
      if (!mjDISABLED(mjDSBL_AUTORESET)) {
        mj_resetData(m, d);
      }
      d->warning[mjWARN_BADQACC].number++;
      d->warning[mjWARN_BADQACC].lastinfo = i;
      if (!mjDISABLED(mjDSBL_AUTORESET)) {
        mj_forward(m, d);
      }
      return;
    }
  }
}


//-------------------------- solver components -----------------------------------------------------

// kinematics-related computations
void mj_fwdKinematics(const mjModel* m, mjData* d) {
  mj_kinematics(m, d);
  mj_comPos(m, d);
  mj_camlight(m, d);
  mj_flex(m, d);
  mj_tendon(m, d);
  if (mj_wakeTendon(m, d)) {
    mj_updateSleep(m, d);
  }
}

// position-dependent computations
void mj_fwdPosition(const mjModel* m, mjData* d) {
  TM_START1;

  // clear position-dependent flags for lazy evaluation
  d->flg_energypos = 0;

  TM_START;
  mj_fwdKinematics(m, d);

  TM_END(mjTIMER_POS_KINEMATICS);

  // inertia, timed internally (POS_INERTIA)
  mj_makeM(m, d);
  mj_factorM(m, d);

  // collision, timed internally (POS_COLLISION)
  mj_collision(m, d);

  if (mj_wakeCollision(m, d)) {
    mj_updateSleep(m, d);
    mj_collision(m, d);
  }

  if (mj_wakeEquality(m, d)) {
    mj_updateSleep(m, d);
  }

  TM_RESTART;
  mj_makeConstraint(m, d);
  mj_island(m, d);
  TM_END(mjTIMER_POS_MAKE);

  TM_RESTART;
  mj_projectConstraint(m, d);
  TM_END(mjTIMER_POS_PROJECT);

  TM_RESTART;
  mj_transmission(m, d);
  TM_ADD(mjTIMER_POS_KINEMATICS);

  // implicit effective metric Mtilde = M + K: build (or deactivate) for this step. Arena
  // lifetime and skip semantics mirror the constraint data: built once per position stage,
  // value-refreshed in the velocity stage, consumed downstream.
  mjd_effBuild(m, d, mj_flexCG(m), /*flg_factor=*/1);

  TM_END1(mjTIMER_POSITION);
}


// velocity-dependent computations
void mj_fwdVelocity(const mjModel* m, mjData* d) {
  TM_START;

  // clear velocity-dependent flags for lazy evaluation
  d->flg_subtreevel = 0;
  d->flg_energyvel = 0;

  // flexedge velocity: skip interp and rigid flexes (edge Jacobians are zero)
  mju_zero(d->flexedge_velocity, m->nflexedge);
  for (int f = 0; f < m->nflex; f++) {
    if (m->flex_rigid[f] || m->flex_interp[f]) continue;
    int adr = m->flex_edgeadr[f];
    int num = m->flex_edgenum[f];
    mju_mulMatVecSparse(d->flexedge_velocity + adr, d->flexedge_J, d->qvel, num,
                        m->flexedge_J_rownnz + adr, m->flexedge_J_rowadr + adr,
                        m->flexedge_J_colind, NULL);
  }

  // tendon velocity: always sparse
  mju_mulMatVecSparse(d->ten_velocity, d->ten_J, d->qvel, m->ntendon,
                      m->ten_J_rownnz, m->ten_J_rowadr, m->ten_J_colind, NULL);

  // actuator velocity: always sparse
  if (!mjDISABLED(mjDSBL_ACTUATION)) {
    mju_mulMatVecSparse(d->actuator_velocity, d->actuator_moment, d->qvel, m->nout,
                        d->moment_rownnz, d->moment_rowadr, d->moment_colind, NULL);
  } else {
    mju_zero(d->actuator_velocity, m->nout);
  }

  // com-based velocities, passive forces, constraint references
  mj_comVel(m, d);
  mj_passive(m, d);
  mj_referenceConstraint(m, d);

  // compute qfrc_bias with abbreviated RNE (without acceleration)
  mj_rne(m, d, 0, d->qfrc_bias);

  // add bias force due to tendon armature
  mj_tendonBias(m, d, d->qfrc_bias);

  mjd_effShift(m, d);

  TM_END(mjTIMER_VELOCITY);
}


// unpack servo-family inputs from control block in canonical order [pos, vel, ff]
// absent input: setpoint 0
static void unpackServoInputs(const mjtNum* u, int spec, mjtNum out[4]) {
  int adr = 0;
  out[0] = (spec & mjINPUT_POS)     ? u[adr++] : 0;
  out[1] = (spec & mjINPUT_VEL)     ? u[adr++] : 0;
  out[2] = (spec & mjINPUT_FF)      ? u[adr++] : 0;
  out[3] = (spec & mjINPUT_VOLTAGE) ? u[adr]   : 0;
}


// helper for DC motor: computes control voltage from PID state
static mjtNum dcmotorVoltage(const mjtNum* u, int spec, mjtNum length, mjtNum velocity,
                             mjtNum x_I, const mjtNum* gainprm) {
  mjtNum voltage = 0;

  // unpack present inputs in canonical order [pos, vel, ff, voltage]; absent input: 0
  mjtNum u4[4];
  unpackServoInputs(u, spec, u4);

  // on-board controller: torque-space PID + torque feedforward
  if (spec & (mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF)) {
    mjtNum kp = gainprm[4];  // proportional gain
    mjtNum ki = gainprm[5];  // integral gain
    mjtNum kd = gainprm[6];  // derivative gain
    mjtNum torque = kp*(u4[0] - length) + kd*(u4[1] - velocity) + ki*x_I + u4[2];

    // torque mode is current control: V = R/K * torque + K * velocity, the second
    // term compensating back-EMF; the compiler requires K > 0 on this path
    mjtNum R = gainprm[0];
    mjtNum K = gainprm[1];
    voltage = R/K * torque + K*velocity;

    // driver supply limit
    mjtNum Vmax = gainprm[7];
    if (Vmax > 0) voltage = mju_clip(voltage, -Vmax, Vmax);
  }

  // raw terminal voltage input: downstream of the controller, unclamped
  return voltage + u4[3];
}


// clamp vector to range
static void clampVec(mjtNum* vec, const mjtNum* range, const mjtBool* limited, int n,
                      const int* index) {
  for (int i=0; i < n; i++) {
    int j = index ? index[i] : i;
    if (limited[i]) {
      vec[j] = mju_clip(vec[j], range[2*i], range[2*i + 1]);
    }
  }
}


// expmap (axis-angle) vector to quaternion
static void expmap2Quat(mjtNum quat[4], const mjtNum v[3]) {
  mjtNum angle = mju_norm3(v);
  if (angle < mjMINVAL) {
    quat[0] = 1;
    quat[1] = quat[2] = quat[3] = 0;
  } else {
    mjtNum axis[3] = {v[0]/angle, v[1]/angle, v[2]/angle};
    mju_axisAngle2Quat(quat, axis, angle);
  }
}


// period of the rotational transmission for wrap-eligible servo actuators, 0 otherwise
static mjtNum wrapPeriod(const mjModel* m, int i) {
  // servo shape: fixed gain, affine bias, matching kp, setpoint input
  mjtDyn dyntype = m->actuator_dyntype[i];
  int servo = m->actuator_gaintype[i] == mjGAIN_FIXED  &&
              m->actuator_biastype[i] == mjBIAS_AFFINE &&
              m->actuator_gainprm[mjNGAIN*i] == -m->actuator_biasprm[mjNBIAS*i+1] &&
              (dyntype == mjDYN_NONE || dyntype == mjDYN_INTEGRATOR);

  // PID shape: kp and kv are single-sourced in the affine bias
  int pid = m->actuator_gaintype[i] == mjGAIN_PID;

  if (!servo && !pid) {
    return 0;
  }

  const mjtNum* gear = m->actuator_gear+6*m->actuator_outadr[i];
  mjtTrn trntype = m->actuator_trntype[i];

  // site transmission with refsite and purely rotational gear
  if (trntype == mjTRN_SITE && m->actuator_trnid[2*i+1] >= 0 &&
      !gear[0] && !gear[1] && !gear[2]) {
    return 2*mjPI * mju_norm3(gear+3);
  }

  // joint transmission on a ball joint
  if ((trntype == mjTRN_JOINT || trntype == mjTRN_JOINTINPARENT) &&
      m->jnt_type[m->actuator_trnid[2*i]] == mjJNT_BALL) {
    return 2*mjPI * mju_norm3(gear);
  }

  return 0;
}


// representative of setpoint u nearest to length, given period
static mjtNum wrapSetpoint(mjtNum u, mjtNum length, mjtNum period) {
  mjtNum err = u - length;
  return u - period * mju_round(err / period);
}


// slew-rate-limit setpoint u given previous effective setpoint u_prev, write act_dot
// period > 0: wrap u to the representative nearest u_prev before limiting
static mjtNum slewLimit(mjtNum u, mjtNum u_prev, mjtNum slew_s, mjtNum dt,
                        mjtNum period, mjtNum* act_dot) {
  if (period > 0) {
    u = wrapSetpoint(u, u_prev, period);
  }
  mjtNum slew = slew_s * dt;
  mjtNum u_eff = mju_clip(u, u_prev - slew, u_prev + slew);
  *act_dot = (u_eff - u_prev) / dt;
  return u_eff;
}


// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) {
  TM_START;
  int nv = m->nv, nu = m->nu, nactuator = m->nactuator, nout = m->nout, ntendon = m->ntendon;
  mjtNum gain, bias, tau;
  mjtNum *force = d->actuator_force;

  // clear actuator_force
  mju_zero(force, nout);

  int sleep_filter = mjENABLED(mjENBL_SLEEP);

  // disabled or no actuation: return
  if (nactuator == 0 || mjDISABLED(mjDSBL_ACTUATION)) {
    mju_zero(d->qfrc_actuator, nv);
    TM_END(mjTIMER_ACTUATION);
    return;
  }

  // any tendon transmission targets with force limits
  int tendon_frclimited = 0;

  // local copy of ctrl
  mj_markStack(d);
  mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum);

  // read from ctrl or history buffer for delayed actuators
  for (int i = 0; i < nactuator; i++) {
    int adr = m->actuator_ctrladr[i];
    if (m->actuator_delay[i]) {
      // delayed: read from history buffer (scalar input)
      int interp = m->actuator_history[2*i+1];
      ctrl[adr] = mj_readCtrl(m, d, i, d->time, interp);
    } else {
      mju_copy(ctrl + adr, d->ctrl + adr, m->actuator_ctrlnum[i]);
    }
  }

  // clamp local copy
  if (!mjDISABLED(mjDSBL_CLAMPCTRL)) {
    clampVec(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL);
  }

  // check controls, set all to 0 if any are bad
  for (int i=0; i < nu; i++) {
    if (mju_isBad(ctrl[i])) {
      mj_warning(d, mjWARN_BADCTRL, i);
      mju_zero(ctrl, nu);
      break;
    }
  }

  // act_dot for stateful actuators
  for (int i=0; i < nactuator; i++) {
    if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
      continue;
    }

    int act_first = m->actuator_actadr[i];
    if (act_first < 0) {
      continue;
    }

    // addresses of the actuator's input and output blocks
    int uadr = m->actuator_ctrladr[i];
    int oadr = m->actuator_outadr[i];

    // zero act_dot for actuator plugins
    int actnum = m->actuator_actnum[i];
    if (actnum) {
      mju_zero(d->act_dot + act_first, actnum);
    }

    // extract info
    const mjtNum* dynprm = m->actuator_dynprm + i*mjNDYN;
    mjtDyn dyntype = m->actuator_dyntype[i];

    // index into the last element in act. For most actuators it's also the
    // first element, but actuator plugins might store their own state in act
    int act_last = act_first + actnum - 1;

    // compute act_dot according to dynamics type
    switch (dyntype) {
    case mjDYN_INTEGRATOR: {        // simple integrator, one per control
      int num = m->actuator_ctrlnum[i];
      for (int j=0; j < num; j++) {
        d->act_dot[act_last-num+1+j] = ctrl[uadr+j];
      }
      break;
    }

    case mjDYN_FILTER:              // linear filter: dynprm = tau
    case mjDYN_FILTEREXACT:
      tau = mju_max(mjMINVAL, dynprm[0]);
      d->act_dot[act_last] = (ctrl[uadr] - d->act[act_last]) / tau;
      break;

    case mjDYN_MUSCLE:              // muscle model: dynprm = (tau_act, tau_deact)
      d->act_dot[act_last] = mju_muscleDynamics(ctrl[uadr], d->act[act_last], dynprm);
      break;

    case mjDYN_PID: {               // PID controller states, slot order: slew, integral
      int adr = act_first;
      mjtNum period = wrapPeriod(m, i);

      // slew rate limiting of the position setpoint
      mjtNum slew_s = dynprm[1];
      if (slew_s > 0) {
        ctrl[uadr] = slewLimit(ctrl[uadr], d->act[adr], slew_s, m->opt.timestep,
                               period, d->act_dot + adr);
        adr++;
      }

      // integral of the position error
      if (m->actuator_gainprm[mjNGAIN*i] > 0) {
        mjtNum err = ctrl[uadr] - d->actuator_length[oadr];

        // rotational transmission: error on the circle
        if (period > 0) {
          err -= period*mju_round(err/period);
        }

        // anti-windup: stop accumulating beyond imax
        mjtNum imax = dynprm[0];
        if (imax > 0) {
          mjtNum z = d->act[adr];
          if (z >= imax) {
            err = mju_min(err, 0);
          } else if (z <= -imax) {
            err = mju_max(err, 0);
          }
        }
        d->act_dot[adr] = err;
      }
      break;
    }

    case mjDYN_DCMOTOR: {           // DC motor: up to 5 optional states
      const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;

      // verify allocated state size matches parameters; SHOULD NOT OCCUR
      if (mj_dcmotorSlots(dynprm, gainprm).num_slots != actnum) {
        mjERROR("inconsistent state array dimension in DC motor (actuator %d)", i);
      }

      int adr = act_first;
      mjtNum velocity = d->actuator_velocity[oadr];
      mjtNum R = gainprm[0];   // resistance
      mjtNum K = gainprm[1];   // motor constant
      mjtNum ki = gainprm[5];  // integral gain
      mjtNum te = dynprm[0];   // electrical time constant

      // slot order: slew, integral, temperature, bristle, current

      // controller state: slew rate limiting
      mjtNum slew_s = dynprm[7];  // slew rate limit
      if (slew_s > 0) {
        ctrl[uadr] = slewLimit(ctrl[uadr], d->act[adr], slew_s, m->opt.timestep,
                               0, d->act_dot + adr);
        adr++;
      }

      // controller state: integral of the position error (setpoint mode only)
      mjtNum x_I = 0;
      if (ki > 0) {
        x_I = d->act[adr];
        mjtNum Imax = dynprm[8];   // integral clamp
        mjtNum act_dot = ctrl[uadr] - d->actuator_length[oadr];

        // clamp act_dot based on integral state
        if (Imax > 0) {
          if (x_I >= Imax) {
            act_dot = mju_min(act_dot, 0);
          } else if (x_I <= -Imax) {
            act_dot = mju_max(act_dot, 0);
          }
        }
        d->act_dot[adr] = act_dot;
        adr++;
      }

      // compute physical voltage to feed into current and temperature equations
      mjtNum V = dcmotorVoltage(ctrl + uadr, m->actuator_ctrlspec[i],
                                d->actuator_length[oadr], velocity, x_I, gainprm);

      // temperature: dT/dt = (R*i^2 - T/RT) / C, where T = delta above ambient
      mjtNum RT = dynprm[2];  // thermal resistance
      if (RT > 0) {
        mjtNum C = dynprm[3];       // thermal capacitance
        mjtNum Ta = dynprm[4];      // ambient temperature
        mjtNum alpha = gainprm[2];  // temperature coefficient
        mjtNum T0 = gainprm[3];     // reference temperature
        mjtNum T = d->act[adr];     // temperature rise above ambient
        R *= 1 + alpha * (T + Ta - T0);

        // get current: from act_last if stateful, from (V - K*omega)/R if stateless
        mjtNum current = (te > 0) ? d->act[act_last] : (V - K * velocity) / R;
        d->act_dot[adr] = (R*current*current - T / RT) / C;
        adr++;
      }

      // LuGre bristle state: dz/dt = v - sigma0 * |v| / g(v) * z
      mjtNum sigma0 = dynprm[5];  // bristle stiffness
      if (sigma0 > 0) {
        const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i;
        mjtNum F_C = biasprm[3];  // Coulomb friction
        mjtNum F_S = biasprm[4];  // static friction
        mjtNum v_S = biasprm[5];  // Stribeck velocity
        mjtNum z = d->act[adr];   // bristle state
        mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S);
        mjtNum a = -sigma0 * mju_abs(velocity) / mju_max(mjMINVAL, g);
        d->act_dot[adr] = a * z + velocity;
        adr++;
      }

      // current state: di/dt = (V/R - K/R*omega - i) / te
      if (te > 0) {
        mjtNum dimax = dynprm[1];  // current rate limit (di/dt)_max
        mjtNum i_dot = (V/R - K/R*velocity - d->act[act_last]) / te;
        if (dimax > 0) {
          i_dot = mju_clip(i_dot, -dimax, dimax);
        }
        d->act_dot[act_last] = i_dot;
      }
      break;
    }

    default:                        // user dynamics
      if (mjcb_act_dyn) {
        if (actnum == 1) {
          // scalar activation dynamics, get act_dot
          d->act_dot[act_last] = mjcb_act_dyn(m, d, i);
        } else {
          // higher-order dynamics, mjcb_act_dyn writes into act_dot directly
          mjcb_act_dyn(m, d, i);
        }
      }
    }
  }

  // get act_dot from actuator 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_ACTUATOR) {
        if (plugin->actuator_act_dot) {
          plugin->actuator_act_dot(m, d, i);
        }
      }
    }
  }

  // force = gain .* [ctrl/act] + bias
  for (int i=0; i < nactuator; i++) {
    // skip if sleeping
    if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
      continue;
    }

    // skip if disabled
    if (mj_actuatorDisabled(m, i)) {
      continue;
    }

    // skip actuator plugins -- these are handled after builtin actuator types
    if (m->actuator_plugin[i] >= 0) {
      continue;
    }

    // addresses of the actuator's input and output blocks
    int uadr = m->actuator_ctrladr[i];
    int oadr = m->actuator_outadr[i];

    // SO(3) geodesic servo: 3 or 4 inputs and 3 outputs on an SO3 transmission
    if (m->actuator_gaintype[i] == mjGAIN_SO3) {
      mjtNum q_tgt[4];

      // quat input: normalize ctrl directly (zero maps to the identity)
      if (m->actuator_ctrlspec[i] == mjCHART_QUAT) {
        mju_copy4(q_tgt, ctrl + uadr);
        mju_normalize4(q_tgt);
      }

      // expmap input: ctrl block (position) or act block (integrator)
      else {
        mjtNum u[3];
        if (m->actuator_dyntype[i] == mjDYN_NONE) {
          mju_copy3(u, ctrl + uadr);
        } else {
          int act_adr = m->actuator_actadr[i];
          if (m->actuator_actearly[i]) {
            for (int k=0; k < 3; k++) {
              u[k] = mj_nextActivation(m, d, i, act_adr+k, d->act_dot[act_adr+k]);
            }
          } else {
            mju_copy3(u, d->act + act_adr);
          }
        }
        expmap2Quat(q_tgt, u);
      }

      // error rotation from current to target: e = log(q_cur^-1 * q_tgt), in the local frame
      // of the transmission, matching the frame of the moment rows and of actuator_velocity
      // note: the force is invariant to the setpoint representative (exp is ray-periodic),
      // so no wrapping is required; act is re-anchored at integration time in mj_advance
      mjtNum q_cur[4], e[3];
      expmap2Quat(q_cur, d->actuator_length + oadr);
      mju_subQuat(e, q_tgt, q_cur);

      // output force: kp * error + constant - kv * velocity
      mjtNum kp = m->actuator_gainprm[mjNGAIN*i];
      const mjtNum* prm = m->actuator_biasprm + mjNBIAS*i;
      for (int k=0; k < 3; k++) {
        force[oadr+k] = kp*e[k] + prm[0] + prm[2]*d->actuator_velocity[oadr+k];
      }
      continue;
    }

    // check for tendon transmission with force limits
    if (ntendon && !tendon_frclimited && m->actuator_trntype[i] == mjTRN_TENDON) {
      tendon_frclimited = m->tendon_actfrclimited[m->actuator_trnid[2*i]];
    }

    // extract info
    const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
    const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
    mjtGain gaintype = m->actuator_gaintype[i];
    int actnum = m->actuator_actnum[i];

    // handle SISO actuators according to gain type
    switch (gaintype) {
    case mjGAIN_FIXED:              // fixed gain: prm = gain
      gain = gainprm[0];
      break;

    case mjGAIN_PID:                // PID servo: input side handled below, state side in bias
      gain = 0;
      break;

    case mjGAIN_AFFINE:             // affine: prm = [const, kp, kv]
      gain = gainprm[0] + gainprm[1]*d->actuator_length[oadr] +
             gainprm[2]*d->actuator_velocity[oadr];
      break;

    case mjGAIN_MUSCLE:             // muscle gain
      gain = mju_muscleGain(d->actuator_length[oadr],
                            d->actuator_velocity[oadr],
                            m->actuator_lengthrange+2*oadr,
                            m->actuator_acc0[oadr],
                            gainprm);
      break;

    case mjGAIN_DCMOTOR: {          // DC motor: gain = K or K/R
      mjtNum R = gainprm[0];  // resistance
      mjtNum K = gainprm[1];  // motor constant
      mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, gainprm);

      // verify allocated state size matches parameters; SHOULD NOT OCCUR
      if (slots.num_slots != actnum) {
        mjERROR("inconsistent state array dimension in DC motor (actuator %d)", i);
      }

      int adr = m->actuator_actadr[i];

      // adjust R for temperature if enabled
      if (slots.temperature >= 0) {
        mjtNum T = d->act[adr + slots.temperature];
        mjtNum alpha = gainprm[2];  // temperature coefficient
        mjtNum T0 = gainprm[3];     // reference temperature
        mjtNum Ta = dynprm[4];      // ambient temperature
        R *= 1 + alpha * (T + Ta - T0);
      }

      // stateful current: gain = K, force = K * act[last] (generic path)
      // stateless: gain = K/R, force = K/R * ctrl (condition below)
      gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R);

      // controller: compute voltage, override ctrl[uadr] for force computation
      // (pure raw-voltage motor reads ctrl directly; empty block reads as 0 below)
      if (m->actuator_ctrlspec[i] != mjINPUT_VOLTAGE && m->actuator_ctrlnum[i] > 0) {
        mjtNum x_I = (slots.integral >= 0) ? d->act[adr + slots.integral] : 0;
        ctrl[uadr] = dcmotorVoltage(ctrl + uadr, m->actuator_ctrlspec[i],
                                    d->actuator_length[oadr],
                                    d->actuator_velocity[oadr], x_I, gainprm);
      }
      break;
    }

    case mjGAIN_SO3:                // handled above via early continue
      mjERROR("mjGAIN_SO3 reached SISO switch (actuator %d)", i);
      break;

    default:                        // user gain
      if (mjcb_act_gain) {
        gain = mjcb_act_gain(m, d, i);
      } else {
        gain = 1;
      }
    }

    // set force = gain .* [ctrl/act]

    // DC motor without current state: use ctrl even if other activations exist
    int dcmotor_no_current = (gaintype == mjGAIN_DCMOTOR && dynprm[0] <= 0);

    // PID servo: force = kp*(qref - l) + kv*(vref - l_dot) [+ ff] [+ ki*z]
    // input-side terms computed here; state-side terms added by the affine bias below
    if (gaintype == mjGAIN_PID) {
      const mjtNum* prm = m->actuator_biasprm + mjNBIAS*i;

      // unpack present inputs in canonical order [pos, vel, ff]; absent input: setpoint 0
      mjtNum u4[4];
      unpackServoInputs(ctrl + uadr, m->actuator_ctrlspec[i], u4);
      mjtNum qref = u4[0], vref = u4[1], ff = u4[2];

      // position setpoint: representative nearest the length on rotational transmissions
      mjtNum period = wrapPeriod(m, i);
      if (period > 0) {
        qref = wrapSetpoint(qref, d->actuator_length[oadr], period);
      }

      // kp and kv are single-sourced in the affine bias parameters
      force[oadr] = -prm[1]*qref - prm[2]*vref + ff;

      // integral state (last slot): force += ki * z
      if (actnum && gainprm[0] > 0) {
        int act_adr = m->actuator_actadr[i] + actnum - 1;
        mjtNum z = m->actuator_actearly[i]
                       ? mj_nextActivation(m, d, i, act_adr, d->act_dot[act_adr])
                       : d->act[act_adr];
        force[oadr] += gainprm[0]*z;
      }
    }
    else if (actnum == 0 || dcmotor_no_current) {
      // empty input block (passive dcmotor): input is 0
      mjtNum input = m->actuator_ctrlnum[i] ? ctrl[uadr] : 0;

      // rotational setpoint: use representative nearest the length (local, no state change)
      mjtNum period = wrapPeriod(m, i);
      if (period > 0) {
        input = wrapSetpoint(input, d->actuator_length[oadr], period);
      }
      force[oadr] = gain * input;
    } else {
      // use last activation variable associated with actuator i
      int act_adr = m->actuator_actadr[i] + actnum - 1;

      mjtNum act;
      if (m->actuator_actearly[i]) {
        act = mj_nextActivation(m, d, i, act_adr, d->act_dot[act_adr]);
      } else {
        act = d->act[act_adr];
      }

      // rotational setpoint: use representative nearest the length (local, no state change)
      mjtNum period = wrapPeriod(m, i);
      if (period > 0) {
        act = wrapSetpoint(act, d->actuator_length[oadr], period);
      }
      force[oadr] = gain * act;
    }

    // extract bias info
    const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i;
    mjtBias biastype = m->actuator_biastype[i];

    // handle according to bias type
    switch (biastype) {
    case mjBIAS_NONE:               // none
      bias = 0.0;
      break;

    case mjBIAS_AFFINE:             // affine: biasprm = [const, kp, kv]
      bias = biasprm[0] + biasprm[1]*d->actuator_length[oadr] +
             biasprm[2]*d->actuator_velocity[oadr];
      break;

    case mjBIAS_MUSCLE:             // muscle passive force
      bias =  mju_muscleBias(d->actuator_length[oadr],
                             m->actuator_lengthrange+2*oadr,
                             m->actuator_acc0[oadr],
                             biasprm);
      break;

    case mjBIAS_DCMOTOR: {          // DC motor: back-EMF only (current-limited)
      bias = 0;

      // back-EMF (stateless only; for stateful current it's in the ODE)
      mjtNum te = m->actuator_dynprm[mjNDYN*i];  // electrical time constant
      if (te <= 0) {
        mjtNum K = gainprm[1];   // motor constant
        bias -= gain * K * d->actuator_velocity[oadr];
      }
      break;
    }

    default:                        // user bias
      if (mjcb_act_bias) {
        bias = mjcb_act_bias(m, d, i);
      } else {
        bias = 0;
      }
    }

    // add bias
    force[oadr] += bias;
  }

  // handle actuator 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_ACTUATOR) {
        if (!plugin->compute) {
          mjERROR("`compute` is a null function pointer for plugin at slot %d", slot);
        }
        plugin->compute(m, d, i, mjPLUGIN_ACTUATOR);
      }
    }
  }

  // clamp tendon total actuator force
  if (tendon_frclimited) {
    // compute total force for each tendon
    mjtNum* tendon_total_force = mjSTACKALLOC(d, ntendon, mjtNum);
    mju_zero(tendon_total_force, ntendon);
    for (int i=0; i < nactuator; i++) {
      if (m->actuator_trntype[i] == mjTRN_TENDON) {
        int tendon_id = m->actuator_trnid[2*i];
        if (m->tendon_actfrclimited[tendon_id]) {
          tendon_total_force[tendon_id] += force[m->actuator_outadr[i]];
        }
      }
    }

    // scale tendon actuator forces if limited and outside range
    for (int i=0; i < nactuator; i++) {
      if (m->actuator_trntype[i] != mjTRN_TENDON) {
        continue;
      }
      int tendon_id = m->actuator_trnid[2*i];
      mjtNum tendon_force = tendon_total_force[tendon_id];
      if (m->tendon_actfrclimited[tendon_id] && tendon_force) {
        const mjtNum* range = m->tendon_actfrcrange + 2 * tendon_id;
        if (tendon_force < range[0]) {
          force[m->actuator_outadr[i]] *= range[0] / tendon_force;
        } else if (tendon_force > range[1]) {
          force[m->actuator_outadr[i]] *= range[1] / tendon_force;
        }
      }
    }
  }

  // clamp actuator_force
  for (int i=0; i < nactuator; i++) {
    if (!m->actuator_forcelimited[i]) {
      continue;
    }
    const mjtNum* range = m->actuator_forcerange + 2*i;
    mjtNum* f = force + m->actuator_outadr[i];

    // SO3: clamp the norm of the output torque, preserving its direction
    if (m->actuator_gaintype[i] == mjGAIN_SO3) {
      mjtNum norm = mju_norm3(f);
      if (norm > range[1]) {
        mju_scl3(f, f, range[1]/norm);
      }
    }

    // otherwise: clamp each output
    else {
      int outnum = m->actuator_outnum[i];
      for (int j=0; j < outnum; j++) {
        f[j] = mju_clip(f[j], range[0], range[1]);
      }
    }
  }

  // add DC motor mechanical forces (not subject to current limits)
  for (int i=0; i < nactuator; i++) {
    if (m->actuator_biastype[i] != mjBIAS_DCMOTOR) {
      continue;
    }
    if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) {
      continue;
    }
    if (mj_actuatorDisabled(m, i) || m->actuator_plugin[i] >= 0) {
      continue;
    }

    const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i;
    const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
    int oadr = m->actuator_outadr[i];

    // cogging torque
    mjtNum A = biasprm[0];
    if (A != 0) {
      mjtNum Np = biasprm[1];
      mjtNum phi = biasprm[2];
      force[oadr] += A * mju_sin(Np*d->actuator_length[oadr] + phi);
    }

    // LuGre friction
    mjtNum sigma0 = dynprm[5];
    if (sigma0 > 0) {
      mjtNum sigma1 = dynprm[6];
      mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, m->actuator_gainprm + mjNGAIN*i);
      int adr = m->actuator_actadr[i] + slots.bristle;
      mjtNum z = d->act[adr];
      mjtNum z_dot = d->act_dot[adr];
      force[oadr] -= sigma0 * z + sigma1 * z_dot;
    }
  }

  // qfrc_actuator = moment' * force
  mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nout, nv,
                       d->moment_rownnz, d->moment_rowadr, d->moment_colind);

  // actuator-level gravity compensation
  if (m->flg_gravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
    // number of dofs for each joint type: {mjJNT_FREE, mjJNT_BALL, mjJNT_SLIDE, mjJNT_HINGE}
    static const int jnt_dofnum[4] = {6, 3, 1, 1};
    int njnt = m->njnt;
    for (int i=0; i < njnt; i++) {
      // skip if gravcomp added as passive force
      if (!m->jnt_actgravcomp[i]) {
        continue;
      }

      // add gravcomp force
      int dofnum = jnt_dofnum[m->jnt_type[i]];
      int dofadr = m->jnt_dofadr[i];
      mju_addTo(d->qfrc_actuator + dofadr, d->qfrc_gravcomp + dofadr, dofnum);
    }
  }

  // clamp qfrc_actuator to joint-level actuator force limits
  clampVec(d->qfrc_actuator, m->jnt_actfrcrange, m->jnt_actfrclimited, m->njnt, m->jnt_dofadr);

  mj_freeStack(d);
  TM_END(mjTIMER_ACTUATION);
}


// add up all non-constraint forces, compute qacc_smooth
void mj_fwdAcceleration(const mjModel* m, mjData* d) {
  int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
  int nv;
  const int* index;

  // qfrc_smooth = qfrc_passive - qfrc_bias + qfrc_applied + qfrc_actuator
  if (!sleep_filter) {
    nv = m->nv;
    index = NULL;
    mju_sub(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, nv);
    mju_addTo(d->qfrc_smooth, d->qfrc_applied, nv);
    mju_addTo(d->qfrc_smooth, d->qfrc_actuator, nv);
  } else {
    nv = d->nv_awake;
    index = d->dof_awake_ind;
    mju_subInd(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, index, nv);
    mju_addToInd(d->qfrc_smooth, d->qfrc_applied, index, nv);
    mju_addToInd(d->qfrc_smooth, d->qfrc_actuator, index, nv);
  }

  // qfrc_smooth += project(xfrc_applied)
  mj_xfrcAccumulate(m, d, d->qfrc_smooth);

  // implicit effective metric (built in mj_fwdPosition): the smooth acceleration is that of
  // the linearly-implicit dynamics, (M + K)*qacc_smooth = qfrc_smooth + c, so the constraint
  // solver, the no-constraint shortcut and the warmstart all see one consistent metric.
  if (d->efm_active) {
    mj_markStack(d);
    mjtNum* qfrc_eff = mjSTACKALLOC(d, nv, mjtNum);
    mju_add(qfrc_eff, d->qfrc_smooth, d->efm_c, nv);
    mjd_effSolve(m, d, d->qacc_smooth, qfrc_eff);
    mj_freeStack(d);
    return;
  }

  // copy for in-place solve: qacc_smooth = qfrc_smooth
  if (!sleep_filter) {
    mju_copy(d->qacc_smooth, d->qfrc_smooth, nv);
  } else {
    mju_copyInd(d->qacc_smooth, d->qfrc_smooth, index, nv);
  }

  // qacc_smooth = M \ qfrc_smooth
  mj_solveLD(d->qacc_smooth, d->qLD, d->qLDiagInv, nv, 1,
             m->M_rownnz, m->M_rowadr, m->M_colind, index);
}


// warmstart/init solver
static void warmstart(const mjModel* m, mjData* d) {
  int nv = m->nv, nefc = d->nefc;

  // warmstart with best of (qacc_warmstart, qacc_smooth)
  if (!mjDISABLED(mjDSBL_WARMSTART)) {
    mj_markStack(d);
    mjtNum* jar = mjSTACKALLOC(d, nefc, mjtNum);

    // start with qacc = qacc_warmstart
    mju_copy(d->qacc, d->qacc_warmstart, nv);

    // compute jar(qacc_warmstart)
    mj_mulJacVec(m, d, jar, d->qacc_warmstart);
    mju_subFrom(jar, d->efc_aref, nefc);

    // update constraints, save cost(qacc_warmstart)
    mjtNum cost_warmstart;
    mj_constraintUpdate(m, d, jar, &cost_warmstart, 0);

    // PGS
    if (m->opt.solver == mjSOL_PGS) {
      // cost(force_warmstart)
      mjtNum PGS_warmstart = mju_dot(d->efc_force, d->efc_b, nefc);
      mjtNum* ARf = mjSTACKALLOC(d, nefc, mjtNum);
      if (mj_isSparse(m))
        mju_mulMatVecSparse(ARf, d->efc_AR, d->efc_force, nefc,
                            d->efc_AR_rownnz, d->efc_AR_rowadr,
                            d->efc_AR_colind, NULL);
      else {
        mju_mulMatVec(ARf, d->efc_AR, d->efc_force, nefc, nefc);
      }
      PGS_warmstart += 0.5*mju_dot(d->efc_force, ARf, nefc);

      // use zero if better
      if (PGS_warmstart > 0) {
        mju_zero(d->efc_force, nefc);
        mju_zero(d->qfrc_constraint, nv);
      }
    }

    // non-PGS
    else {
      // add Gauss to cost(qacc_warmstart)
      mjtNum* Ma = mjSTACKALLOC(d, nv, mjtNum);
      mj_mulM(m, d, Ma, d->qacc_warmstart);
      for (int i=0; i < nv; i++) {
        cost_warmstart += 0.5*(Ma[i]-d->qfrc_smooth[i])*(d->qacc_warmstart[i]-d->qacc_smooth[i]);
      }

      // cost(qacc_smooth)
      mjtNum cost_smooth;
      mj_constraintUpdate(m, d, d->efc_b, &cost_smooth, 0);

      // use qacc_smooth if better
      if (cost_warmstart > cost_smooth) {
        mju_copy(d->qacc, d->qacc_smooth, nv);
      }
    }

    // have island structure: unconstrained qacc = qacc_smooth
    if (d->nisland > 0) {
      // loop over unconstrained dofs in map_idof2dof[nidof, nv)
      for (int i=d->nidof; i < nv; i++) {
        int dof = d->map_idof2dof[i];
        d->qacc[dof] = d->qacc_smooth[dof];
      }
    }

    mj_freeStack(d);
  }

  // coldstart with qacc = qacc_smooth, efc_force = 0
  else {
    mju_copy(d->qacc, d->qacc_smooth, nv);
    mju_zero(d->efc_force, nefc);
  }
}


// mju_dispatch callback: solve one island
static void solveIslandTask(const mjModel* m, mjData* d, void* arg, int thread_id, int island) {
  if (m->opt.solver == mjSOL_NEWTON) {
    mj_solNewton_island(m, d, island, m->opt.iterations);
  } else if (m->opt.solver == mjSOL_CG) {
    mj_solCG_island(m, d, island, m->opt.iterations);
  } else {
    mj_solPGS_island(m, d, island, m->opt.iterations);
  }
}


// compute efc_b, efc_force, qfrc_constraint; update qacc
void mj_fwdConstraint(const mjModel* m, mjData* d) {
  TM_START;
  int nv = m->nv, nefc = d->nefc, nisland = d->nisland, nidof;

  // always clear qfrc_constraint
  mju_zero(d->qfrc_constraint, nv);

  // no constraints: copy unconstrained acc, clear forces, return
  // (with the effective metric active, qacc_smooth is already the implicit answer)
  if (!nefc) {
    mju_copy(d->qacc, d->qacc_smooth, nv);
    mju_zeroInt(d->solver_niter, mjNISLAND);
    TM_END(mjTIMER_CONSTRAINT);
    return;
  }

  // compute efc_b = J*qacc_smooth - aref
  mj_mulJacVec(m, d, d->efc_b, d->qacc_smooth);
  mju_subFrom(d->efc_b, d->efc_aref, nefc);

  // check for invalid solver type
  if (m->opt.solver != mjSOL_PGS && m->opt.solver != mjSOL_CG && m->opt.solver != mjSOL_NEWTON) {
    mjERROR("unknown solver type %d", m->opt.solver);
  }

  // warmstart solver
  warmstart(m, d);
  mju_zeroInt(d->solver_niter, mjNISLAND);

  // check if islands are supported
  // TODO: support islands with the implicit effective metric and remove the mj_flexCG
  // condition. It is here because the metric machinery is monolithic: the efm_c shift and
  // the Ma/Mv/Mgrad operators (mjd_effMulAdd, mjd_effPrec) act on global dof vectors with
  // no island-local form. Discovery is already handled: findEdges unions the trees of every
  // stiffness-active flex, so a flex always lands in one island together with everything it
  // touches. Removal therefore needs only the solver side: apply the efm_c shift to that
  // island's dofs, gather/scatter its island-local vectors around the covered-compact
  // factor solves (the factors themselves need no change), and enable the metric path
  // (flg_flex) for the flex-containing island alone.
  int islands_supported = !mjDISABLED(mjDSBL_ISLAND) && nisland > 0 && !mj_flexCG(m);

  // run solver over constraint islands
  if (islands_supported) {
    switch ((mjtSolver) m->opt.solver) {
    case mjSOL_PGS:
      mju_dispatch(m, d, solveIslandTask, NULL, nisland);
      break;

    case mjSOL_CG:
    case mjSOL_NEWTON:
      // copy inputs to islands (vel+acc deps, pos-dependent already copied in mj_island)
      nidof = d->nidof;
      mju_gather(d->ifrc_smooth,     d->qfrc_smooth,     d->map_idof2dof, nidof);
      mju_gather(d->ifrc_constraint, d->qfrc_constraint, d->map_idof2dof, nidof);
      mju_gather(d->iacc_smooth,     d->qacc_smooth,     d->map_idof2dof, nidof);
      mju_gather(d->iacc,            d->qacc,            d->map_idof2dof, nidof);
      mju_gather(d->iefc_force,      d->efc_force,       d->map_iefc2efc, nefc);
      mju_gather(d->iefc_aref,       d->efc_aref,        d->map_iefc2efc, nefc);

      mju_dispatch(m, d, solveIslandTask, NULL, nisland);

      // copy back solver outputs (scatter dofs since ni <= nv)
      mju_scatter(d->qacc,            d->iacc,            d->map_idof2dof, nidof);
      mju_scatter(d->qfrc_constraint, d->ifrc_constraint, d->map_idof2dof, nidof);
      mju_gather(d->efc_force, d->iefc_force, d->map_efc2iefc, nefc);
      break;
    }

    // run noslip solver per island if enabled
    if (m->opt.noslip_iterations > 0) {
      for (int island=0; island < nisland; island++) {
        mj_solNoSlip_island(m, d, island, m->opt.noslip_iterations);
      }
    }
  }

  // run solver over all constraints (monolithic)
  else {
    switch ((mjtSolver) m->opt.solver) {
    case mjSOL_PGS:                     // PGS
      mj_solPGS(m, d, m->opt.iterations);
      break;

    case mjSOL_CG:                      // CG
      mj_solCG(m, d, m->opt.iterations);
      break;

    case mjSOL_NEWTON:                  // Newton
      mj_solNewton(m, d, m->opt.iterations);
      break;
    }

    // run noslip solver if enabled
    if (m->opt.noslip_iterations > 0) {
      mj_solNoSlip(m, d, m->opt.noslip_iterations);
    }
  }

  // dual solvers: map efc_force to joint space (always monolithic)
  if (m->opt.solver == mjSOL_PGS || m->opt.noslip_iterations > 0) {
    mj_dualFinish(m, d);
  }

  TM_END(mjTIMER_CONSTRAINT);
}


//-------------------------- state advancement and integration  ------------------------------------

// advance state and time
//   act_dot: activation derivatives
//   qacc:    acceleration used to update d->qvel (d->qvel += h*qacc)
//   qvel:    optional velocity used for position integration; if NULL, use d->qvel
static void mj_advance(const mjModel* m, mjData* d,
                       const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) {
  int nactuator = m->nactuator, nsensor = m->nsensor;

  // advance history buffers
  if (m->nhistory > 0) {
    // advance ctrl history buffers
    for (int i = 0; i < nactuator; i++) {
      int nsample = m->actuator_history[2*i];
      if (nsample == 0) continue;

      // get history buffer pointer and insert ctrl at current time
      mjtNum* buf = d->history + m->actuator_historyadr[i];
      *mju_historyInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[m->actuator_ctrladr[i]];
    }

    // advance sensor history buffers
    for (int i = 0; i < nsensor; i++) {
      int nsample = m->sensor_history[2*i];
      if (nsample == 0) continue;

      // get history buffer parameters
      int dim = m->sensor_dim[i];
      mjtNum* buf = d->history + m->sensor_historyadr[i];
      mjtNum delay = m->sensor_delay[i];
      mjtNum interval = m->sensor_interval[2*i];

      if (interval > 0) {
        // interval mode: if condition is satisfied, compute; otherwise copy
        mjtNum time_prev = buf[0];  // first slot stores previous sensor tick
        if (time_prev + interval <= d->time) {
          buf[0] += interval;  // advance by exact interval (continuous time)
          mjtNum* slot = mju_historyInsert(buf, nsample, dim, d->time);
          if (delay > 0) {
            // have delay, compute sensor
            mj_computeSensor(m, d, i, slot);
          } else {
            // no delay, copy from sensordata (already computed)
            mju_copy(slot, d->sensordata + m->sensor_adr[i], dim);
          }
        }
      } else if (delay > 0) {
        // delay-only mode: always compute and insert
        mjtNum* slot = mju_historyInsert(buf, nsample, dim, d->time);
        mj_computeSensor(m, d, i, slot);
      } else {
        // history-only mode: copy from sensordata (already computed)
        mjtNum* slot = mju_historyInsert(buf, nsample, dim, d->time);
        mju_copy(slot, d->sensordata + m->sensor_adr[i], dim);
      }
    }
  }

  // advance activations
  if (m->na && !mjDISABLED(mjDSBL_ACTUATION)) {
    for (int i=0; i < nactuator; i++) {
      int actadr = m->actuator_actadr[i];
      int actadr_end = actadr + m->actuator_actnum[i];
      for (int j=actadr; j < actadr_end; j++) {
        // if disabled, set act_dot to 0
        d->act[j] = mj_nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]);
      }
    }

    // rotational setpoints stored in act: replace with an equivalent bounded representative,
    // like the actrange clamp above, this is a projection applied at integration time
    for (int i=0; i < nactuator; i++) {
      if (m->actuator_dyntype[i] != mjDYN_INTEGRATOR) {
        continue;
      }

      // per-axis servo: wrap act to the representative nearest the length
      mjtNum period = wrapPeriod(m, i);
      if (period > 0) {
        int adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1;
        d->act[adr] = wrapSetpoint(d->act[adr], d->actuator_length[m->actuator_outadr[i]], period);
      }

      // SO3 servo: re-anchor the act setpoint to the canonical representative
      else if (m->actuator_gaintype[i] == mjGAIN_SO3) {
        int adr = m->actuator_actadr[i];
        mjtNum angle = mju_norm3(d->act + adr);
        if (angle > mjPI) {
          mjtNum scale = (angle - 2*mjPI*mju_round(angle/(2*mjPI))) / angle;
          for (int k=0; k < 3; k++) {
            d->act[adr+k] *= scale;
          }
        }
      }
    }
  }

  // put islands to sleep according to velocity tolerance
  if (mj_sleep(m, d)) {
    // if any trees put to sleep (qvel set to 0), recompute all velocity-dependent quantities
    mj_forwardSkip(m, d, mjSTAGE_POS, 0);

    // update sleep indices
    mj_updateSleep(m, d);
  }

  // advance velocities
  int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree;
  if (sleep_filter) {
    mju_addToSclInd(d->qvel, qacc, d->dof_awake_ind, m->opt.timestep, d->nv_awake);
  } else {
    mju_addToScl(d->qvel, qacc, m->opt.timestep, m->nv);
  }

  // advance positions with qvel if given, d->qvel otherwise (semi-implicit)
  const int* index = sleep_filter ? d->body_awake_ind : NULL;
  int nbody = sleep_filter ? d->nbody_awake : m->nbody;
  mj_integratePosInd(m, d->qpos, qvel ? qvel : d->qvel, m->opt.timestep, index, nbody);

  // advance time
  d->time += m->opt.timestep;

  // advance plugin states
  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->advance) {
        plugin->advance(m, d, i);
      }
    }
  }

  // save qacc for next step warmstart
  mju_copy(d->qacc_warmstart, d->qacc, m->nv);
}

// Euler integrator, semi-implicit in velocity, possibly skipping factorisation
void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
  TM_START;
  mj_markStack(d);
  mjtNum* qfrc = mjSTACKALLOC(d, m->nv, mjtNum);
  mjtNum* qacc = mjSTACKALLOC(d, m->nv, mjtNum);

  // sleep filtering
  int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
  int nv = sleep_filter ? d->nv_awake : m->nv;
  const int* dof_awake_ind = sleep_filter ? d->dof_awake_ind : NULL;

  // check for dof damping if disable flag is not set
  int dof_damping = 0;
  if (!mjDISABLED(mjDSBL_EULERDAMP) && !mjDISABLED(mjDSBL_DAMPER)) {
    for (int v=0; v < nv; v++) {
      int i = sleep_filter ? dof_awake_ind[v] : v;
      if (m->dof_damping[i] > 0 ||
          !mju_isZero(m->dof_dampingpoly + mjNPOLY*i, mjNPOLY) ||
          m->jnt_actuatorid[m->dof_jntid[i]] != -1) {
        dof_damping = 1;
        break;
      }
    }
  }

  // no damping or disabled: explicit velocity integration
  if (!dof_damping) {
    if (sleep_filter) {
      mju_copyInd(qacc, d->qacc, dof_awake_ind, nv);
    } else {
      mju_copy(qacc, d->qacc, nv);
    }
  }

  // damping: integrate implicitly
  else {
    if (!skipfactor) {
      // qH = M
      if (sleep_filter) {
        mju_copySparse(d->qH, d->M, m->M_rownnz, m->M_rowadr, dof_awake_ind, d->nv_awake);
      } else {
        mju_copy(d->qH, d->M, m->nC);
      }

      // qH += h*diag(B)
      for (int v=0; v < nv; v++) {
        int i = sleep_filter ? dof_awake_ind[v] : v;
        mjtNum qv = d->qvel[i];
        mjtNum poly[mjNPOLY];
        mju_copy(poly, m->dof_dampingpoly + mjNPOLY*i, mjNPOLY);
        mjtNum damping = m->dof_damping[i]
                         + mj_actuatorDamping(m, mjOBJ_JOINT, m->dof_jntid[i], poly);
        mjtNum damp_deriv = mjd_xPolyForce(damping, poly, qv, mjNPOLY, 1);
        d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * damp_deriv;
      }

      // factorize in-place
      mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind);
    }

    // solve
    if (sleep_filter) {
      mju_addInd(qfrc, d->qfrc_smooth, d->qfrc_constraint, dof_awake_ind, nv);
      mju_copyInd(qacc, qfrc, dof_awake_ind, nv);
    } else {
      mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
      mju_copy(qacc, qfrc, nv);
    }
    mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
               m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind);
  }

  // advance state and time
  mj_advance(m, d, d->act_dot, qacc, NULL);

  mj_freeStack(d);

  TM_END(mjTIMER_ADVANCE);
}


// Euler integrator, semi-implicit in velocity
void mj_Euler(const mjModel* m, mjData* d) {
  mj_EulerSkip(m, d, 0);
}


// RK4 tableau
const mjtNum RK4_A[9] = {
  0.5,    0,      0,
  0,      0.5,    0,
  0,      0,      1
};

const mjtNum RK4_B[4] = {
  1.0/6.0, 1.0/3.0, 1.0/3.0, 1.0/6.0
};


// Runge Kutta explicit order-N integrator
//  (A,B) is the tableau, C is set to row_sum(A)
void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
  int nv = m->nv, nq = m->nq, na = m->na;
  mjtNum h = m->opt.timestep, time = d->time;
  mjtNum C[9], T[9], *X[10], *F[10], *dX;
  const mjtNum* A = (N == 4 ? RK4_A : 0);
  const mjtNum* B = (N == 4 ? RK4_B : 0);

  // check order
  if (!A) {
    mjERROR("supported RK orders: N=4");
  }

  // allocate space for intermediate solutions
  mj_markStack(d);
  dX = mjSTACKALLOC(d, 2*nv+na, mjtNum);
  for (int i=0; i < N; i++) {
    X[i] = mjSTACKALLOC(d, nq+nv+na, mjtNum);
    F[i] = mjSTACKALLOC(d, nv+na, mjtNum);
  }

  // precompute C and T;  C,T,A have size (N-1)
  for (int i=1; i < N; i++) {
    // C(i) = sum_j A(i,j)
    C[i-1] = 0;
    for (int j=0; j < i; j++) {
      C[i-1] += A[(i-1)*(N-1)+j];
    }

    // compute T
    T[i-1] = d->time + C[i-1]*h;
  }

  // init X[0], F[0]; mj_forward() was already called
  mju_copy(X[0], d->qpos, nq);
  mju_copy(X[0]+nq, d->qvel, nv);
  mju_copy(F[0], d->qacc, nv);
  if (na) {
    mju_copy(X[0]+nq+nv, d->act, na);
    mju_copy(F[0]+nv, d->act_dot, na);
  }

  // compute the remaining X[i], F[i]
  for (int i=1; i < N; i++) {
    // compute dX
    mju_zero(dX, 2*nv+na);
    for (int j=0; j < i; j++) {
      mju_addToScl(dX, X[j]+nq, A[(i-1)*(N-1)+j], nv);
      mju_addToScl(dX+nv, F[j], A[(i-1)*(N-1)+j], nv+na);
    }

    // compute X[i] = X[0] '+' dX
    mju_copy(X[i], X[0], nq+nv+na);
    mj_integratePos(m, X[i], dX, h);
    mju_addToScl(X[i]+nq, dX+nv, h, nv+na);

    // set X[i], T[i-1] in mjData
    mju_copy(d->qpos, X[i], nq);
    mju_copy(d->qvel, X[i]+nq, nv);
    if (na) {
      mju_copy(d->act, X[i]+nq+nv, na);
    }
    d->time = T[i-1];

    // evaluate F[i]
    mj_forwardSkip(m, d, mjSTAGE_NONE, 1);  // 1: do not recompute sensors and energy
    mju_copy(F[i], d->qacc, nv);
    if (na) {
      mju_copy(F[i]+nv, d->act_dot, na);
    }
  }

  // compute dX for final update (using B instead of A)
  mju_zero(dX, 2*nv+na);
  for (int j=0; j < N; j++) {
    mju_addToScl(dX, X[j]+nq, B[j], nv);
    mju_addToScl(dX+nv, F[j], B[j], nv+na);
  }

  // reset state and time
  d->time = time;
  mju_copy(d->qpos, X[0], nq);
  mju_copy(d->qvel, X[0]+nq, nv);
  mju_copy(d->act, X[0]+nq+nv, na);

  // advance state and time
  mj_advance(m, d, dX+2*nv, dX+nv, dX);

  mj_freeStack(d);
}


// return 1 if any flex needs implicit stiffness treatment (interp or bending)
// return 1 if any non-rigid flex uses passive contacts (needs the metric independently of elasticity)
static mjtBool flex_has_passive_contact(const mjModel* m) {
  for (int f=0; f < m->nflex; f++) {
    if (!m->flex_rigid[f] && m->flex_passive[f]) {
      return 1;
    }
  }
  return 0;
}

static mjtBool flex_has_implicit_stiffness(const mjModel* m) {
  for (int f=0; f < m->nflex; f++) {
    if (m->flex_rigid[f]) {
      continue;
    }

    // interpolated flex with stiffness
    if (m->flex_interp[f] &&
        m->flex_edgeequality[f] != 3 &&
        m->flex_stiffness[m->flex_stiffnessadr[f]] != 0) {
      return 1;
    }

    // standard flex with bending
    if (!m->flex_interp[f] && m->flex_dim[f] == 2 &&
        m->flex_bendingadr[f] >= 0) {
      return 1;
    }

    // standard flex with stretch
    if (!m->flex_interp[f] && m->flex_dim[f] >= 2 &&
        m->flex_stiffnessadr[f] >= 0 &&
        m->flex_stiffness[m->flex_stiffnessadr[f]] != 0) {
      return 1;
    }
  }
  return 0;
}


// implicit-flex solve gate: with the CG solver, an implicit integrator and flex stiffness
// present, the CG solve carries the implicit flex stiffness itself -- K = (h^2+h*d) times the flex stiffness enters
// the objective/gradient/linesearch, and the preconditioned gradient becomes (M+K)\grad by
// linear matrix-free CG against the existing M factor (the in-solver form of the old post-hoc
// flexInterp_cgsolve treatment, no factorization anywhere); mj_implicitSkip then folds the
// implicit flex force of the solver's qacc into qfrc. When active with islands
// enabled, mj_fwdConstraint forces a monolithic solve (flex mesh coupling is invisible to
// constraint islanding). solver="Newton" keeps its exact-factorization semantics untouched.
// Models outside the gate integrate flex elasticity explicitly.
int mj_flexCG(const mjModel* m) {
  return m->opt.solver == mjSOL_CG &&
         (m->opt.integrator == mjINT_IMPLICIT || m->opt.integrator == mjINT_IMPLICITFAST) &&
         m->opt.cone != mjCONE_ELLIPTIC && !mjENABLED(mjENBL_SLEEP) &&
         (flex_has_implicit_stiffness(m) || flex_has_passive_contact(m));
}


// fully implicit in velocity, possibly skipping factorization
void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
  TM_START;
  int nD = m->nD, nC = m->nC, njnt = m->njnt;

  mj_markStack(d);
  mjtNum* qfrc = mjSTACKALLOC(d, m->nv, mjtNum);
  mjtNum* qacc = mjSTACKALLOC(d, m->nv, mjtNum);

  // sleep filtering
  int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nv_awake < m->nv;
  int nv = sleep_filter ? d->nv_awake : m->nv;
  const int* dof_awake_ind = sleep_filter ? d->dof_awake_ind : NULL;

  // set qfrc = qfrc_smooth + qfrc_constraint
  if (sleep_filter) {
    mju_addInd(qfrc, d->qfrc_smooth, d->qfrc_constraint, dof_awake_ind, nv);
  } else {
    mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
  }

  // implicit flex stiffness is carried by the constraint solver (see mj_flexCG): use the
  // solver's qacc directly. The qDeriv treatment is skipped for these models -- flex damping
  // is already implicit inside the solve (the s2 terms of B), joint damping and other velocity
  // derivatives integrate explicitly. This avoids both the qDeriv machinery and the
  // sequential flex-vs-qDeriv splitting. Models outside the gate (non-Newton solver, elliptic
  // cones, islands, sleep) integrate flex elasticity explicitly.
  int flexcg = !sleep_filter && mj_flexCG(m);

  // factorization
  if (!skipfactor && !flexcg) {
    // implicit
    if (m->opt.integrator == mjINT_IMPLICIT) {
      // compute analytical derivative qDeriv
      mjd_smooth_vel(m, d, /* flg_bias = */ 1);

      // gather qLU <- M (lower to full)
      mju_gatherMasked(d->qLU, d->M, m->mapM2D, nD);

      // set qLU = M - dt*qDeriv
      mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, nD);
    }

    // implicitfast
    else if (m->opt.integrator == mjINT_IMPLICITFAST) {
      // compute analytical derivative qDeriv; skip rne derivative
      mjd_smooth_vel(m, d, /* flg_bias = */ 0);

      // modified mass matrix: gather qH <- qDeriv (full to lower)
      mju_gather(d->qH, d->qDeriv, m->mapD2M, nC);

      // set qH = M - dt*qDeriv
      mju_addScl(d->qH, d->M, d->qH, -m->opt.timestep, nC);

      // standalone free bodies: reset qH block rows to M; their qDeriv rows may be asymmetric and
      // are handled by the local LU solve; we reset to keep LTL well-defined
      for (int j=0; j < njnt; j++) {
        if (m->jnt_type[j] != mjJNT_FREE || !mj_isFreeBody(m, m->jnt_bodyid[j])) {
          continue;
        }
        int adr = m->jnt_dofadr[j];
        for (int r=0; r < 6; r++) {
          mju_copy(d->qH + m->M_rowadr[adr+r], d->M + m->M_rowadr[adr+r], m->M_rownnz[adr+r]);
        }
      }
    } else {
      mjERROR("integrator must be implicit or implicitfast");
    }

    // standard factorization (implicit / implicitfast)
    if (m->opt.integrator == mjINT_IMPLICIT) {
      int* scratch = mjSTACKALLOC(d, nv, int);
      mju_factorLUSparse(d->qLU, nv, scratch, m->D_rownnz, m->D_rowadr, m->D_colind, dof_awake_ind);
    } else {
      mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind);
    }
  }

  // standard sparse solve
  if (flexcg) {
    // constraint solver's qacc already carries the implicit flex force
    mju_copy(qacc, d->qacc, m->nv);
  } else if (m->opt.integrator == mjINT_IMPLICIT) {
    mju_solveLUSparse(qacc, d->qLU, qfrc, nv, m->D_rownnz, m->D_rowadr, m->D_diag, m->D_colind,
                      dof_awake_ind);
  } else {
    // implicitfast
    if (sleep_filter) {
      mju_copyInd(qacc, qfrc, dof_awake_ind, nv);
    } else {
      mju_copy(qacc, qfrc, nv);
    }
    mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind);
  }

  // implicitfast: local unsymmetric solve for standalone free bodies
  // adds the bias (gyroscopic) derivative, dropped from the global symmetric solve; the
  // 6x6 block of M - h*D is decoupled from the rest of the system (D sparsity is tree-local),
  // so overwriting these rows of qacc leaves all other DOFs unaffected
  if (m->opt.integrator == mjINT_IMPLICITFAST && !flexcg) {
    for (int j=0; j < m->njnt; j++) {
      mjtNum A[36];
      if (!mjd_freeMhat(m, d, j, m->opt.timestep, A)) {
        continue;
      }

      // solve A * qacc_block = qfrc_block
      int adr = m->jnt_dofadr[j];
      int pivot[6];
      if (mju_factorLU6(A, pivot)) {
        mjtNum x[6];  // local vector for guaranteed memory alignment
        mju_solveLU6(x, A, qfrc+adr, pivot);
        mji_copy6(qacc+adr, x);
      }
    }
  }

  // advance state and time
  mj_advance(m, d, d->act_dot, qacc, NULL);

  mj_freeStack(d);

  TM_END(mjTIMER_ADVANCE);
}


// fully implicit in velocity
void mj_implicit(const mjModel* m, mjData* d) {
  mj_implicitSkip(m, d, 0);
}


//-------------------------- top-level API ---------------------------------------------------------

// forward dynamics with skip; skipstage is mjtStage
void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) {
  TM_START;

  // Passive flex contact is too stiff for explicit integration; require the effective metric.
  if (flex_has_passive_contact(m) && !mj_flexCG(m)) {
    mjERROR("passive flex contact requires the effective metric: use integrator=\"implicit\" or "
            "\"implicitfast\" with solver=\"CG\", pyramidal cones and sleep disabled");
  }

  // position-dependent
  if (skipstage < mjSTAGE_POS) {
    mj_fwdPosition(m, d);

    if (!skipsensor) {
      mj_sensorPos(m, d);
    }

    if (!d->flg_energypos) {
      if (mjENABLED(mjENBL_ENERGY)) {
        mj_energyPos(m, d);
      } else {
        d->energy[0] = d->energy[1] = 0;
      }
    }
  }

  // velocity-dependent
  if (skipstage < mjSTAGE_VEL) {
    mj_fwdVelocity(m, d);

    if (!skipsensor) {
      mj_sensorVel(m, d);
    }

    if (mjENABLED(mjENBL_ENERGY) && !d->flg_energyvel) {
      mj_energyVel(m, d);
    }
  }

  // acceleration-dependent
  if (mjcb_control && !mjDISABLED(mjDSBL_ACTUATION)) {
    mjcb_control(m, d);
  }

  mj_fwdActuation(m, d);
  mj_fwdAcceleration(m, d);
  mj_fwdConstraint(m, d);
  if (!skipsensor) {
    d->flg_rnepost = 0;  // clear flag for lazy evaluation
    mj_sensorAcc(m, d);
  }

  TM_END(mjTIMER_FORWARD);
}


// forward dynamics
void mj_forward(const mjModel* m, mjData* d) {
  mj_forwardSkip(m, d, mjSTAGE_NONE, 0);
}


// advance simulation using control callback
void mj_step(const mjModel* m, mjData* d) {
  TM_START;

  // common to all integrators
  mj_checkPos(m, d);
  mj_checkVel(m, d);
  mj_forward(m, d);
  mj_checkAcc(m, d);

  // compare forward and inverse solutions if enabled
  if (mjENABLED(mjENBL_FWDINV)) {
    mj_compareFwdInv(m, d);
  }

  // use selected integrator
  switch ((mjtIntegrator) m->opt.integrator) {
  case mjINT_EULER:
    mj_Euler(m, d);
    break;

  case mjINT_RK4:
    mj_RungeKutta(m, d, 4);
    break;

  case mjINT_IMPLICIT:
  case mjINT_IMPLICITFAST:
    mj_implicit(m, d);
    break;

  default:
    mjERROR("invalid integrator");
  }

  TM_END(mjTIMER_STEP);
}


// advance simulation in two phases: before input is set by user
void mj_step1(const mjModel* m, mjData* d) {
  TM_START;
  mj_checkPos(m, d);
  mj_checkVel(m, d);
  mj_fwdPosition(m, d);
  mj_sensorPos(m, d);

  if (!d->flg_energypos) {
    if (mjENABLED(mjENBL_ENERGY)) {
      mj_energyPos(m, d);
    } else {
      d->energy[0] = d->energy[1] = 0;
    }
  }

  mj_fwdVelocity(m, d);
  mj_sensorVel(m, d);
  if (mjENABLED(mjENBL_ENERGY) && !d->flg_energyvel) {
    mj_energyVel(m, d);
  }

  if (mjcb_control) {
    mjcb_control(m, d);
  }
  TM_END(mjTIMER_STEP);
}


//   >>>>   user can modify ctrl and q/xfrc_applied between step1 and step2   <<<<


// advance simulation in two phases: after input is set by user
void mj_step2(const mjModel* m, mjData* d) {
  TM_START;
  mj_fwdActuation(m, d);
  mj_fwdAcceleration(m, d);
  mj_fwdConstraint(m, d);
  d->flg_rnepost = 0;  // clear flag for lazy evaluation
  mj_sensorAcc(m, d);
  mj_checkAcc(m, d);

  // compare forward and inverse solutions if enabled
  if (mjENABLED(mjENBL_FWDINV)) {
    mj_compareFwdInv(m, d);
  }

  // integrate with Euler or implicit; RK4 defaults to Euler
  if (m->opt.integrator == mjINT_IMPLICIT || m->opt.integrator == mjINT_IMPLICITFAST) {
    mj_implicit(m, d);
  } else {
    mj_Euler(m, d);
  }

  d->timer[mjTIMER_STEP].number--;
  TM_END(mjTIMER_STEP);
}