ogym 0.1.0

Composable, type-safe reinforcement learning environments for Rust
Documentation
use super::config::HalfCheetahConfig;
use crate::env::environment::{Environment, Experience, Terminal};
use crate::env::{environment::Error, mujoco::mjenv::MjEnv};
use nalgebra::DVector;
use rand::{rngs::StdRng, Rng, SeedableRng};
use rand_distr::StandardNormal;
use std::collections::HashMap;

pub type Action = DVector<f64>;
pub type State = DVector<f64>;
pub type Info = HashMap<String, f64>;

pub struct MujocoHalfCheetahEnv {
    pub env: MjEnv,
    config: HalfCheetahConfig,
    init_qpos: Vec<f64>,
    init_qvel: Vec<f64>,
    step_count: usize,
}

impl MujocoHalfCheetahEnv {
    pub fn new(config: Option<HalfCheetahConfig>) -> Result<Self, Error> {
        let config = config.unwrap_or_default();
        let env = MjEnv::new(config.xml(), config.frame_skip())?;
        let init_qpos = env.init_qpos().to_vec();
        let init_qvel = env.init_qvel().to_vec();

        Ok(Self {
            env,
            config,
            init_qpos,
            init_qvel,
            step_count: 0,
        })
    }

    fn _get_observation(&self) -> Result<State, Error> {
        let qpos = self.env.qpos();
        let qvel = self.env.qvel();

        let pos_start_idx = if self.config.exclude_current_positions_from_observation() {
            1
        } else {
            0
        };

        let obs_dim = (qpos.len() - pos_start_idx) + qvel.len();
        let mut observation = Vec::with_capacity(obs_dim);

        observation.extend_from_slice(&qpos[pos_start_idx..]);
        observation.extend_from_slice(qvel);

        Ok(DVector::from_vec(observation))
    }

    fn _compute_reward(&self, x_velocity: f64, action: &Action) -> (f64, Info) {
        let forward_reward = self.config.forward_reward_weight() * x_velocity;

        let squared_action_sum: f64 = action.iter().map(|a| a.powi(2)).sum();
        let ctrl_cost = self.config.ctrl_cost_weight() * squared_action_sum;

        let total_reward = forward_reward - ctrl_cost;

        let mut info = HashMap::new();
        info.insert("reward_forward".to_string(), forward_reward);
        info.insert("reward_ctrl".to_string(), -ctrl_cost);

        (total_reward, info)
    }

    fn _get_reset_info(&self) -> Info {
        let mut info = HashMap::new();
        if let Some(x_pos) = self.env.qpos().first() {
            info.insert("x_position".to_string(), *x_pos);
        }
        info
    }
}

impl Environment for MujocoHalfCheetahEnv {
    type Action = Action;
    type State = State;
    type Info = Info;

    fn reset(&mut self, seed: Option<u64>) -> Result<(Self::State, Self::Info), Error> {
        self.step_count = 0;
        self.env.reset_to_initial()?;

        let mut rng = match seed {
            Some(s) => StdRng::seed_from_u64(s),
            None => StdRng::from_os_rng(),
        };

        let noise_scale = self.config.reset_noise_scale();

        let qpos_noisy: Vec<f64> = if noise_scale > 0.0 {
            self.init_qpos
                .iter()
                .map(|&val| {
                    let noise = rng.random_range(-noise_scale..noise_scale);
                    val + noise
                })
                .collect()
        } else {
            self.init_qpos.clone()
        };

        let qvel_noisy: Vec<f64> = if noise_scale > 0.0 {
            self.init_qvel
                .iter()
                .map(|&val| {
                    let noise: f64 = rng.sample(StandardNormal);
                    val + noise * noise_scale
                })
                .collect()
        } else {
            self.init_qvel.clone()
        };

        self.env.set_state(&qpos_noisy, &qvel_noisy)?;

        Ok((self._get_observation()?, self._get_reset_info()))
    }

    fn step(
        &mut self,
        action: Self::Action,
    ) -> Result<Experience<Self::State, Self::Info, Self::Action>, Error> {
        let current_state = self._get_observation()?;
        let x_pos_before = self.env.qpos()[0];

        self.env.do_simulation(action.as_slice())?;
        self.step_count += 1;

        let x_pos_after = self.env.qpos()[0];

        let dt = self.env.dt();
        let x_velocity = (x_pos_after - x_pos_before) / dt;
        let (reward, mut info) = self._compute_reward(x_velocity, &action);

        let next_state = self._get_observation()?;

        let terminated = false;
        let truncated = self.step_count >= self.config.max_episode_steps();

        info.insert("x_position".to_string(), x_pos_after);
        info.insert("x_velocity".to_string(), x_velocity);

        let terminal = Terminal::from_flags(terminated, truncated);

        Ok(Experience::new(
            current_state,
            reward,
            action,
            next_state,
            info,
            terminal,
            self.step_count as u32,
        ))
    }

    fn is_terminal(&self) -> Result<bool, Error> {
        Ok(false)
    }

    fn is_truncated(&self) -> bool {
        self.step_count >= self.config.max_episode_steps()
    }

    fn state(&self) -> Result<Self::State, Error> {
        self._get_observation()
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;

    const TEST_XML: &str = r#"
    <mujoco model="cheetah_test">
        <compiler angle="radian"/>
        <option timestep="0.01" gravity="0 0 -9.81"/>
        <worldbody>
            <body name="torso" pos="0 0 0">
                <joint name="rootx" type="slide" axis="1 0 0" />
                <joint name="rootz" type="slide" axis="0 0 1" />
                <joint name="rooty" type="hinge" axis="0 1 0" />
                <geom type="capsule" size="0.046" fromto="-.5 0 0 .5 0 0" />
            </body>
        </worldbody>
        <actuator>
             <motor name="m1" joint="rootx" gear="1" />
        </actuator>
    </mujoco>
    "#;

    fn get_test_config() -> HalfCheetahConfig {
        HalfCheetahConfig::new()
            .with_xml(TEST_XML)
            .with_frame_skip(1)
            .with_reset_noise_scale(0.0)
    }

    #[test]
    fn test_env_initialization() {
        let config = get_test_config();
        let env = MujocoHalfCheetahEnv::new(Some(config));
        assert!(env.is_ok(), "Environment should initialize with valid XML");

        let env = env.unwrap();
        assert_eq!(env.env.nq(), 3);
        assert_eq!(env.env.nu(), 1);
    }

    #[test]
    fn test_reset_determinism_and_noise() {
        let mut config = get_test_config();
        config = config.with_reset_noise_scale(0.0);
        let mut env = MujocoHalfCheetahEnv::new(Some(config.clone())).unwrap();

        let (obs1, _) = env.reset(Some(42)).unwrap();
        let (obs2, _) = env.reset(Some(99)).unwrap();

        assert_relative_eq!(obs1[0], env.init_qpos[1]);
        assert_eq!(obs1, obs2);

        config = config.with_reset_noise_scale(0.5);
        let mut env_noisy = MujocoHalfCheetahEnv::new(Some(config)).unwrap();

        let (obs_seed_a, _) = env_noisy.reset(Some(123)).unwrap();
        let (obs_seed_a_again, _) = env_noisy.reset(Some(123)).unwrap();
        let (obs_seed_b, _) = env_noisy.reset(Some(456)).unwrap();
        assert_relative_eq!(obs_seed_a, obs_seed_a_again);
        assert_ne!(obs_seed_a, obs_seed_b);
    }

    #[test]
    fn test_observation_dimensions() {
        let config = get_test_config().with_position_exclusion(true);
        let mut env = MujocoHalfCheetahEnv::new(Some(config)).unwrap();
        let (obs, _) = env.reset(None).unwrap();

        assert_eq!(obs.len(), 5, "Observation vector dimension mismatch");
    }

    #[test]
    fn test_step_logic_and_reward() {
        let config = get_test_config()
            .with_forward_reward_weight(1.0)
            .with_ctrl_cost_weight(0.1);

        let mut env = MujocoHalfCheetahEnv::new(Some(config)).unwrap();
        env.reset(Some(1)).unwrap();

        let action = DVector::from_vec(vec![1.0]);
        let experience = env.step(action.clone()).unwrap();

        let next_x = experience.info.get("x_position").unwrap();
        assert!(
            *next_x > 0.0,
            "Agent should move forward with positive action"
        );

        let x_vel = experience.info.get("x_velocity").unwrap();
        let fwd_reward = 1.0 * x_vel;
        let ctrl_cost = 0.1 * (1.0f64.powi(2));
        let expected_reward = fwd_reward - ctrl_cost;

        assert_relative_eq!(experience.reward, expected_reward, epsilon = 1e-6);
        assert!(!experience.terminal.is_terminated());
    }

    #[test]
    fn test_truncation_at_max_episode_steps() {
        let config = get_test_config().with_max_episode_steps(5);
        let mut env = MujocoHalfCheetahEnv::new(Some(config)).unwrap();
        env.reset(None).unwrap();

        let action = DVector::from_vec(vec![0.0]);
        for _ in 0..4 {
            let exp = env.step(action.clone()).unwrap();
            assert!(!exp.terminal.is_truncated());
        }
        let exp = env.step(action).unwrap();
        assert!(exp.terminal.is_truncated());
        assert!(env.is_truncated());
    }

    #[test]
    fn test_action_bounds_check() {
        let mut env = MujocoHalfCheetahEnv::new(Some(get_test_config())).unwrap();
        env.reset(None).unwrap();

        let bad_action = DVector::from_vec(vec![1.0, 2.0, 3.0]);
        let result = env.step(bad_action);

        assert!(
            result.is_err(),
            "Should return error for wrong action dimension"
        );
    }
}