mujoco传感器数据读取的正确姿势

之前一直以为mujoco的仿真环境下六维力传感器的数据读取方式C++版本如下

void printForceTorqueData(const mjModel *m, const mjData *d)
{
    int force_id = mj_name2id(m, mjOBJ_SENSOR, "fts_sensor_force");
    int torque_id = mj_name2id(m, mjOBJ_SENSOR, "fts_sensor_torque");
    if (force_id != -1 && torque_id != -1)
    {
        Eigen::Map<Eigen::Vector3d> force(&d->sensordata[force_id], 3);
        Eigen::Map<Eigen::Vector3d> torque(&d->sensordata[torque_id], 3);
        std::cerr << "Force: " << force.transpose() << std::endl;
        std::cerr << "Torque: " << torque.transpose() << std::endl;
    }
}

而经过与mujoco的Python接口对比，发现上述方式读取的力矩信息是错的，需要改成如下的读取方式

void printForceTorqueData(const mjModel *m, const mjData *d)
{
    int force_id = mj_name2id(m, mjOBJ_SENSOR, "fts_sensor_force");
    int torque_id = mj_name2id(m, mjOBJ_SENSOR, "fts_sensor_torque");
    if (force_id != -1 && torque_id != -1)
    {
        // 获取传感器数据起始索引
        int force_adr = m->sensor_adr[force_id];
        int torque_adr = m->sensor_adr[torque_id];
        Eigen::Vector3d force_local(d->sensordata[force_adr], d->sensordata[force_adr+1], d->sensordata[force_adr+2]);
        Eigen::Vector3d torque_local(d->sensordata[torque_adr], d->sensordata[torque_adr+1], d->sensordata[torque_adr+2]);
        std::cerr << "force_local: " << force_local.transpose() << std::endl;
        std::cerr << "torque_local: " << torque_local.transpose() << std::endl;
    }
}