77
88from unilab .base import registry
99from unilab .base .backend import create_backend
10+ from unilab .base .dtype_config import get_global_dtype
1011from unilab .base .np_env import NpEnvState
1112from unilab .envs .locomotion .go2 .base import Go2BaseCfg , Go2BaseEnv
1213from unilab .utils .math_utils import np_quat_mul , np_yaw_to_quat
@@ -83,6 +84,7 @@ def __init__(self, cfg: Go2JoystickCfg, num_envs=1, backend_type="mujoco"):
8384 backend_type , cfg .model_file , num_envs , cfg .sim_dt , base_name = cfg .asset .base_name
8485 )
8586 super ().__init__ (cfg , backend , num_envs )
87+ self ._enable_reward_log = True
8688 self ._init_reward_functions ()
8789
8890 @property
@@ -105,76 +107,83 @@ def _init_reward_functions(self):
105107 }
106108
107109 def update_state (self , state : NpEnvState ) -> NpEnvState :
108- terminated = self ._compute_termination ()
109- reward = self ._compute_reward (state .info )
110- obs = self ._compute_obs (state .info )
111- return state .replace (obs = obs , reward = reward , terminated = terminated )
112-
113- def _compute_obs (self , info : dict ) -> dict [str , np .ndarray ]:
114110 linvel = self .get_local_linvel ()
115111 gyro = self .get_gyro ()
116112 gravity = self ._backend .get_sensor_data ("upvector" )
117113 dof_pos = self .get_dof_pos ()
118114 dof_vel = self .get_dof_vel ()
115+ terminated = gravity [:, 2 ] <= 0.5
116+ reward = self ._compute_reward (state .info , linvel , gyro , dof_pos )
117+ obs = self ._compute_obs (state .info , linvel , gyro , gravity , dof_pos , dof_vel )
118+ return state .replace (obs = obs , reward = reward , terminated = terminated )
119119
120+ def _compute_obs (
121+ self , info : dict , linvel , gyro , gravity , dof_pos , dof_vel
122+ ) -> dict [str , np .ndarray ]:
120123 diff = dof_pos - self .default_angles
121124 command = info ["commands" ]
122125 last_actions = info .get ("current_actions" , np .zeros_like (diff ))
123-
124- actor = np .concatenate ([gyro , - gravity , diff , dof_vel , last_actions , command ], axis = 1 )
126+ actor = np .concatenate (
127+ [gyro , - gravity , diff , dof_vel , last_actions , command ],
128+ axis = 1 ,
129+ dtype = get_global_dtype (),
130+ )
125131 return {"actor" : actor , "privileged" : linvel }
126132
127- def _compute_reward (self , info : dict ) -> np .ndarray :
128- reward = np .zeros ((self ._num_envs ,), dtype = np .float32 )
133+ def _compute_reward (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
134+ dtype = get_global_dtype ()
135+ reward = np .zeros ((self ._num_envs ,), dtype = dtype )
129136 cfg = self ._cfg .reward_config
130137
138+ step_count = info .get ("steps" , np .zeros ((self ._num_envs ,), dtype = np .uint32 ))
139+ should_log = self ._enable_reward_log and (int (step_count [0 ]) % 4 == 0 )
140+ log = {} if should_log else info .get ("log" , {})
141+
131142 for name , scale in cfg .scales .items ():
132143 if scale == 0 or name not in self ._reward_fns :
133144 continue
134- rew = self ._reward_fns [name ](info )
135- reward + = rew * scale
136- if "log" not in info :
137- info [ "log" ] = {}
138- info [ " log" ] [f"reward/{ name } " ] = float (np .mean (rew * scale ))
145+ rew = self ._reward_fns [name ](info , linvel , gyro , dof_pos )
146+ weighted_rew = rew * scale
147+ reward += weighted_rew
148+ if should_log :
149+ log [f"reward/{ name } " ] = float (np .mean (weighted_rew ))
139150
140- reward *= self . _cfg . ctrl_dt
141- return reward
151+ info [ "log" ] = log
152+ return reward * self . _cfg . ctrl_dt
142153
143- def _reward_tracking_lin_vel (self , info : dict ) -> np .ndarray :
144- linvel = self .get_local_linvel ()
154+ # ── reward functions ──────────────────────────────────────────────
155+
156+ def _reward_tracking_lin_vel (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
145157 commands = info ["commands" ]
146158 lin_vel_error = np .sum (np .square (commands [:, :2 ] - linvel [:, :2 ]), axis = 1 )
147159 return np .asarray (np .exp (- lin_vel_error / self ._cfg .reward_config .tracking_sigma ))
148160
149- def _reward_tracking_ang_vel (self , info : dict ) -> np .ndarray :
150- gyro = self .get_gyro ()
161+ def _reward_tracking_ang_vel (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
151162 commands = info ["commands" ]
152163 ang_vel_error = np .square (commands [:, 2 ] - gyro [:, 2 ])
153164 return np .asarray (np .exp (- ang_vel_error / self ._cfg .reward_config .tracking_sigma ))
154165
155- def _reward_lin_vel_z (self , info : dict ) -> np .ndarray :
156- global_linvel = self ._backend .get_sensor_data ("global_linvel" )
157- return np .asarray (np .square (global_linvel [:, 2 ]))
166+ def _reward_lin_vel_z (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
167+ return np .asarray (np .square (linvel [:, 2 ]))
158168
159- def _reward_ang_vel_xy (self , info : dict ) -> np .ndarray :
160- gyro = self .get_gyro ()
169+ def _reward_ang_vel_xy (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
161170 return np .asarray (np .sum (np .square (gyro [:, :2 ]), axis = 1 ))
162171
163- def _reward_base_height (self , info : dict ) -> np .ndarray :
172+ def _reward_base_height (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
164173 base_height = self ._backend .get_base_pos ()[:, 2 ]
165174 return np .asarray (np .square (base_height - self ._cfg .reward_config .base_height_target ))
166175
167- def _reward_action_rate (self , info : dict ) -> np .ndarray :
176+ def _reward_action_rate (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
168177 action_diff = info ["current_actions" ] - info ["last_actions" ]
169178 return np .asarray (np .sum (np .square (action_diff ), axis = 1 ))
170179
171- def _reward_similar_to_default (self , info : dict ) -> np .ndarray :
172- return np .asarray (np .sum (np .abs (self . get_dof_pos () - self .default_angles ), axis = 1 ))
180+ def _reward_similar_to_default (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
181+ return np .asarray (np .sum (np .abs (dof_pos - self .default_angles ), axis = 1 ))
173182
174- def _reward_alive (self , info : dict ) -> np .ndarray :
175- return np .ones ((self ._num_envs ,), dtype = np . float32 )
183+ def _reward_alive (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
184+ return np .ones ((self ._num_envs ,), dtype = get_global_dtype () )
176185
177- def _reward_foot_lift (self , info : dict ) -> np .ndarray :
186+ def _reward_foot_lift (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
178187 foot_pos = self .get_foot_pos ()
179188 foot_heights = foot_pos [..., 2 ]
180189 foot_contact = self .get_foot_contact ()
@@ -185,7 +194,7 @@ def _reward_foot_lift(self, info: dict) -> np.ndarray:
185194 reward = np .exp (- error_sq / sigma ) * is_swing
186195 return np .asarray (np .sum (reward , axis = 1 ))
187196
188- def _reward_foot_drag (self , info : dict ) -> np .ndarray :
197+ def _reward_foot_drag (self , info : dict , linvel , gyro , dof_pos ) -> np .ndarray :
189198 foot_pos = self .get_foot_pos ()
190199 foot_heights = foot_pos [..., 2 ]
191200 foot_contact = self .get_foot_contact ()
@@ -195,18 +204,18 @@ def _reward_foot_drag(self, info: dict) -> np.ndarray:
195204 error = np .square (height_error ) * is_swing
196205 return np .asarray (np .sum (error , axis = 1 ))
197206
198- def _compute_termination (self ) -> np .ndarray :
199- gravity = self ._backend .get_sensor_data ("upvector" )
200- return np .asarray (gravity [:, 2 ] < 0.5 )
201-
202207 def reset (self , env_indices : np .ndarray ):
203208 num_reset = len (env_indices )
204209 qpos = np .tile (self ._init_qpos , (num_reset , 1 ))
205210 qvel = np .tile (self ._init_qvel , (num_reset , 1 ))
206211
212+ # Domain Randomization
213+ dxy = np .random .uniform (- 0.5 , 0.5 , (num_reset , 2 ))
214+ qpos [:, 0 :2 ] += dxy
207215 yaw = np .random .uniform (- np .pi , np .pi , (num_reset ,))
208216 quat_yaw = np_yaw_to_quat (yaw )
209- qpos [:, 3 :7 ] = np_quat_mul (quat_yaw , qpos [:, 3 :7 ])
217+ qpos [:, 3 :7 ] = np_quat_mul (qpos [:, 3 :7 ], quat_yaw )
218+ qvel [:, 0 :6 ] = np .random .uniform (- 0.5 , 0.5 , (num_reset , 6 ))
210219
211220 self ._backend .set_state (env_indices , qpos , qvel )
212221
@@ -217,11 +226,15 @@ def reset(self, env_indices: np.ndarray):
217226 )
218227
219228 info = {
220- "commands" : np .zeros ((self ._num_envs , 3 ), dtype = np .float32 ),
221- "current_actions" : np .zeros ((self ._num_envs , self ._num_action ), dtype = np .float32 ),
229+ "commands" : commands ,
230+ "current_actions" : np .zeros ((num_reset , self ._num_action ), dtype = get_global_dtype ()),
231+ "last_actions" : np .zeros ((num_reset , self ._num_action ), dtype = get_global_dtype ()),
222232 }
223- info ["commands" ][env_indices ] = commands
224233
225- obs = self ._compute_obs (info )
226- obs_subset = {k : v [env_indices ] for k , v in obs .items ()}
227- return obs_subset , obs_subset , {k : v [env_indices ] for k , v in info .items ()}
234+ linvel = self .get_local_linvel ()[env_indices ]
235+ gyro = self .get_gyro ()[env_indices ]
236+ gravity = self ._backend .get_sensor_data ("upvector" )[env_indices ]
237+ dof_pos = self .get_dof_pos ()[env_indices ]
238+ dof_vel = self .get_dof_vel ()[env_indices ]
239+ obs = self ._compute_obs (info , linvel , gyro , gravity , dof_pos , dof_vel )
240+ return obs , obs , info
0 commit comments