Can't update the robot's state with data->variable1 = variable2.

[parkhy0106]

System Information

/0/b memory 40GiB System Memory /0/b/0 memory 8GiB SODIMM DDR4 Synchronous /0/b/1 memory 32GiB SODIMM DDR4 Synchronous /0/d memory 512KiB L1 cache /0/e memory 4MiB L2 cache /0/f memory 16MiB L3 cache /0/10 processor AMD Ryzen 7 5800H with Radeon

code sniffet

stand.h

...
class YP : public Task {
 public:
  std::string Name() const override;
  std::string XmlPath() const override;

  class ResidualFn : public BaseResidualFn {
   public:
    explicit ResidualFn(const YP* task, int current_mode = 0)
        : BaseResidualFn(task), current_mode_(current_mode) {}
    // ------- Residuals for OP3 task ------------
    //     Residual(0): height - feet height
    //     Residual(1): balance
    //     Residual(2): center of mass xy velocity
    //     Residual(3): ctrl - ctrl_nominal
    //     Residual(4): upright
    //     Residual(5): joint velocity
    // -------------------------------------------
    void Residual(const mjModel* model, const mjData* data,
                  double* residual) const override;

   private:
    friend class YP;
    int current_mode_;

    // modes
    enum OP3Mode {
      kModeStand = 0,
      kModeHandstand,
    };
  };

  YP() : residual_(this) {}
  void TransitionLocked(mjModel* model, mjData* data) override;

  // default height goals
  constexpr static double kModeHeight[2] = {0.38, 0.57};

 protected:
  std::unique_ptr<mjpc::ResidualFn> ResidualLocked() const override {
    return std::make_unique<ResidualFn>(this, residual_.current_mode_);
  }
  ResidualFn* InternalResidual() override { return &residual_; }

 private:
  ResidualFn residual_;
};
...

stand.cc

...
  // ----- Actuators ---- //
  double* hip_r = SensorByName(model, data, "HIP_R");
  double* hip_l = SensorByName(model, data, "HIP_L");
  double* thigh_r = SensorByName(model, data, "THIGH_R");
  double* thigh_l = SensorByName(model, data, "THIGH_L");
  double* shin_r = SensorByName(model, data, "SHIN_R");
  double* shin_l = SensorByName(model, data, "SHIN_L");
  double* foot_r = SensorByName(model, data, "FOOT_R");
  double* foot_l = SensorByName(model, data, "FOOT_L");

  int hip_r_joint_id = mj_name2id(model, mjOBJ_JOINT, "HIP_R");

  data->qpos[hip_r_joint_id] = 1.57;  // 라디안 단위
  data->qvel[hip_r_joint_id] = 0;  // 라디안/초
  hip_r[2] = 1.57;
...

task.xml

...
    <jointpos name="HIP_R"   joint="HR"/>
    <jointpos name="HIP_L"   joint="HR"/>
    <jointpos name="THIGH_R" joint="TR"/>
    <jointpos name="THIGH_L" joint="HR"/>
    <jointpos name="SHIN_R"  joint="SR"/>
    <jointpos name="SHIN_L"  joint="SL"/>
    <jointpos name="FOOT_R"  joint="FR"/>
    <jointpos name="FOOT_L"  joint="FL"/>
...

Is there any proper way to update the robot state while simulating? What I'm trying to do is to update the real-world robot state to the simulation. Whenever I try to update the state by changing the data(or usually named as d) variable, the joint doesn't change and the simulator's state gets unstable.

[thowell]

The simulation state can be modified using Task::TransitionLocked (example).

The mjData object passed to this function is for the simulation environment, not the planner.

[parkhy0106]

The simulation state can be modified using Task::TransitionLocked (example).

The mjData object passed to this function is for the simulation environment, not the planner.

I already tried it and yes. I can change the simulation environment, and as you mentioned, I couldn't change the parameter for the planner smoothly.

So if it's okay, Can I ask you where the planner gets the mjData? Is there any kind of entry point can I access? I know this is abnormal. Usually doing this thing for implementing planner to robot is not right, but I have to make the GUI as fast as I can and I think this is the only way I can do so far.

[thowell]

At this line the simulation mjData is copied into a State object that is then passed to the planner. You could modify this object, which contains the correct mjData.