Update sim/base_sim.py

sim/base_sim.py

@@ -86,6 +86,8 @@ class DefaultEnv:
             str(assets_root / self.config["ROBOT_SCENE"])
         )
         self.mj_data = mujoco.MjData(self.mj_model)
+        # Set valid floating base quaternion (MjData initializes qpos to zeros)
+        self.mj_data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
         self.mj_model.opt.timestep = self.sim_dt
         self.torso_index = mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_BODY, "torso_link")
         self.root_body = "pelvis"
@@ -502,6 +504,11 @@ class DefaultEnv:
 
     def reset(self):
         mujoco.mj_resetData(self.mj_model, self.mj_data)
+        # Set valid floating base quaternion (identity: w=1, x=y=z=0)
+        # mj_resetData sets qpos to zeros, which gives invalid [0,0,0,0] quaternion
+        self.mj_data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
+        # Propagate qpos to derived quantities (xquat, xpos, etc.)
+        mujoco.mj_forward(self.mj_model, self.mj_data)
 
 
 class BaseSimulator: