fix phase

Author: sWintermoor

src/bitbots_motion/bitbots_rl_motion/bitbots_rl_motion/phase.py

@@ -8,16 +8,22 @@ class PhaseObject:
     _phase_dt: float
 
     def __init__(self, config):
-
         self._control_dt = config["phase"]["control_dt"]
         self._gait_frequency = config["phase"]["gait_frequency"]
         self._phase_dt = 2 * np.pi * self._gait_frequency * self._control_dt
+        self._obs_phase = None
 
     def set_phase(self, new_phase):
         self._phase = new_phase
 
+    def set_obs_phase(self, new_obs_phase):
+        self._obs_phase = new_obs_phase
+
     def get_phase(self):
         return self._phase
 
     def get_phase_dt(self):
         return self._phase_dt
+
+    def get_obs_phase(self):
+        return self._obs_phase

src/bitbots_motion/bitbots_rl_motion/nodes/kick_node.py

@@ -46,18 +46,20 @@ def _ball_pos_callback(self, msg):
 
     # observations
     def obs(self):
-        return np.hstack(
+        observation = np.hstack(
             [
                 self._gyro_handler.get_gyro(),
                 self._gravity_handler.get_gravity(),
                 self._joint_handler.get_velocity_data(),
                 self._joint_handler.get_angle_data(),
                 self._previous_action.get_previous_action(),
-                self._phase.get_phase(),
+                self._phase.get_obs_phase(),
                 self._ball_handler.get_ball_pos(),
             ]
         ).astype(np.float32)
 
+        return observation
+
     # load phase function
     def load_phase(self):
         pass

src/bitbots_motion/bitbots_rl_motion/nodes/rl_node.py

@@ -62,15 +62,17 @@ def _timer_callback(self):
 
         # TODO consider IMU mounting offset
 
-        self._phase.set_phase(
+        self._phase.set_obs_phase(
             np.array(
                 [np.cos(self._phase.get_phase()), np.sin(self._phase.get_phase())],
                 dtype=np.float32,
             ).flatten()
         )
 
+        observation = self.obs()
+
         # Run the ONNX model
-        onnx_input = {self._onnx_input_name[0]: self.obs().reshape(1, -1)}  # TODO: Improve input
+        onnx_input = {self._onnx_input_name[0]: observation.reshape(1, -1)}  # TODO: Improve input
         onnx_pred = self._onnx_session.run(self._onnx_output_name, onnx_input)[0][0]
         self._previous_action.set_previous_action(onnx_pred)
 

src/bitbots_motion/bitbots_rl_motion/nodes/walk_node.py

@@ -56,7 +56,7 @@ def obs(self):
                 self._joint_handler.get_velocity_data(),
                 self._joint_handler.get_angle_data(),
                 self._previous_action.get_previous_action(),
-                self._phase.get_phase(),
+                self._phase.get_obs_phase(),
             ]
         ).astype(np.float32)