How can I add sub stepping into the simulation?

[MyelinsheathXD]

Hello everybody! Recently I have managed to optimize the Humanoid Tracking task to run in real time (on 16 thread cpu ) by increasing physics time step size from 0.002 sec upto 0.016 sec . However the large time step made the simulation unstable during large scaled perturbations. My goal is to run MujocoMPC as efficient as possible for real time usage.

Right now. I am trying to optimize MPC problem solving time by one of these options:

1. adding physics 10x step physics simulation substepping after every method in main MPC architecture's "mj_step()" calls
2. decision request from MPC at every 10x physics step rather than at every 1x physics step.

So far I have tried these methods to achive this optimization:

1. Modifiying mujoco engine source to add internal physics engine sub stepping into the method "mj_step()". After this implamentation, MPC can not solve the problem and generated poorly solved action sequences.

2. Adding extra sub stepping next to the main "mj_step()" methods in MPC architechture in files trajectories.cc, app.cc

trajectory.cc app.cc

//trajectory.cc file
//line number 169
   // record state
    mju_copy(DataAt(states, (t + 1) * dim_state), data->qpos, nq);
    mju_copy(DataAt(states, (t + 1) * dim_state + nq), data->qvel, nv);
    mju_copy(DataAt(states, (t + 1) * dim_state + nq + nv), data->act, na);
    times[t + 1] = data->time;
           //------------ my sub stepping modification------------
    for (size_t ie = 0; ie < 10; ie++)
    {
      // step
      mj_step(model, data);
    }

//line number 179

//trajectory.cc file
//line number 274
    // record state
    mju_copy(DataAt(states, (t + 1) * dim_state), data->qpos, nq);
    mju_copy(DataAt(states, (t + 1) * dim_state + nq), data->qvel, nv);
    mju_copy(DataAt(states, (t + 1) * dim_state + nq + nv), data->act, na);
    times[t + 1] = data->time;

           //------------ my sub stepping modification------------
    for (size_t ie = 0; ie < 10; ie++)
    {
      // step
      mj_step(model, data);
    }
//line number 286

also modified in app.cc file

//app.cc file
//line number 328
            // run single step, let next iteration deal with timing
            sim.agent->ExecuteAllRunBeforeStepJobs(m, d);
            mj_step(m, d);

           // ------------my sub stepping modification----------
            for (size_t ie = 0; ie < 10; ie++)
            {
              // step
              mj_step(m, d);
            } 

//app.cc file
//line number 328
              // call mj_step
              sim.agent->ExecuteAllRunBeforeStepJobs(m, d);
              mj_step(m, d);
           //------------ my sub stepping modification------------
              for (size_t ie = 0; ie < 10; ie++)
              {
                // step
                mj_step(m, d);
              }

I have tried not to touch app.cc at all. Since mpc policy has "data->time" variabled for every trajectory . However after several combinational changes of these codes above, iLQG MPC produced poorly generated control action sequences.

What is the best way to call MPC trajectory desicision request not in every 1x physics simulation but in every 10x physics simulation? How can I implament it in Mujoco_MPC?

[thowell]

The GUI application utilizes an asynchronous planner. The ratio of simulation to planning step will be dependent on your hardware.

If you would like to run the simulation and planner synchronously, this can be achieved using the Python API.