how to set mujoco robot state with real robot state values?

[starimpact]

Intro

Hi!

I am a ([graduate / undergrad] student / professor / researcher) at XYZ, I use MuJoCo for my research on ABC.

My setup

i have a real robot, and i create a counterpart of it in mujoco simulation.
i collect real robot joints sensors values in realtime with serial communication, and send them to mujoco, and then using these values to change simulation's robot qpos and sensor values, i want to get the synchronization effect between real robot and simulation robot.
i do these value settings in mujoco estimator loop in app.cc of mujoco_mpu.

My question

current situation and problem: simulation robot can stands up when real robot stands up, and squats also works. but the simulation robot always floating and moving towards a direction.
as following image, the robot penetrates into the ground.

Minimal model and/or code that explain my question

No response

Confirmations

• I searched the latest documentation thoroughly before posting.
• I searched previous Issues and Discussions, I am certain this has not been raised before.

[yuvaltassa]

What you want is called an estimator, i.e. something that answers the question "what is the most likely simulation state given the current sensor values and the previous estimate". We have some estimator implementations in the MJPC project, but they are not strictly part of MuJoCo.

[starimpact]

I do know the mjpc and kalman estimator, and I add code into the EstimatorLoop of MJPC.
the following is a piece from app.cc of mjpc.
only change sensor value of mujoco is not helpful, change qpos value directly can get some positive visual effect in mujoco viewer, but follows strange phenomenon(floating and moving).

void processSensorData(const mjModel* model, mjData* data, DataPacket& packet) {
    /*
    float dataArray[arraySize]; //0:time 
                      //1~6: angX, angY, angZ, gyroX, gyroY, gyroZ, 
                      //7~10: motor1_shaft_angle, motor2_shaft_angle, motor1_shaft_velocity, motor2_shaft_velocity
                      //11~12: thigh_Left, thigh_Right
    */
    // 假设 float_array 包含欧拉角（以弧度为单位）
    mjtNum euler[3] = {packet.float_array[0], packet.float_array[1], packet.float_array[2]};
    mjtNum quat[4];

    // transform degree to radian
    for (int i = 0; i < 3; ++i) {
        euler[i] = euler[i] * 3.14 / 180;
    }

    // 将欧拉角转换为四元数
    mju_euler2Quat(quat, euler, "XYZ");

    set_sensor_by_name(model, data, "est_root_quat", quat);

    double* root_joint = mjpc::JointQPosByName(model, data, "root_joint");
    std::cout << "root_joint: ";
    for (int i = 0; i < 7; ++i) {
        std::cout << root_joint[i] << " ";
    }
    std::cout << std::endl;

    // copy quaternion to root_joint
    mju_copy(root_joint+3, quat, 4);


    float thigh_left = packet.float_array[10];
    float thigh_right = packet.float_array[11];
    mjtNum sim_thigh_left = 0;
    mjtNum sim_thigh_right = 0;
    mapping_thighs_position(thigh_left, thigh_right, sim_thigh_left, sim_thigh_right);
    std::cout << "mapping_thighs_position: " << sim_thigh_left << " " << sim_thigh_right << std::endl;
    // get joint qpos data using string
    double* thigh_left_joint = mjpc::JointQPosByName(model, data, "thigh_left_joint");
    double* thigh_right_joint = mjpc::JointQPosByName(model, data, "thigh_right_joint");
    std::cout << "thigh_left_joint: " << thigh_left_joint[0] << ", thigh_right_joint: " << thigh_right_joint[0] << std::endl;
    thigh_left_joint[0] = sim_thigh_left;
    thigh_right_joint[0] = sim_thigh_right;
}

// estimator in background thread
void EstimatorLoop(mj::Simulate& sim) {
  // run until asked to exit
  while (!sim.exitrequest.load()) {
    if (sim.uiloadrequest.load() == 0) {
      // estimator
      int active_estimator = sim.agent->ActiveEstimatorIndex();
      mjpc::Estimator* estimator = &sim.agent->ActiveEstimator();

      // estimator update
      if (!active_estimator) {
        std::this_thread::yield();
        continue;
      } else {
        // start timer
        auto start = std::chrono::steady_clock::now();

        // set values from GUI
        estimator->SetGUIData();

        // get simulation state (lock physics thread)
        {
          const std::lock_guard<std::mutex> lock(sim.mtx);
          
          // copy simulation ctrl
          mju_copy(sim.agent->ctrl.data(), d->ctrl, m->nu);
          {
            // get serial data
            std::cout << "================hello...serial_receive.get_packet================" << std::endl;
            std::cout << "EstimatorLoop 0:";
            print_sensor_by_name(m, d, "est_root_quat");
            DataPacket packet;
            while (true) {
              
              if (serial_receive.get_packet(packet)) {
                std::cout << "d address: " << d << ", ms: " << packet.ms << ", float_array: ";
                for (float value : packet.float_array) {
                    std::cout << value << " ";
                }
                std::cout << std::endl;
                processSensorData(m, d, packet);
                break;
              }
            }
          }