ogym 0.1.0

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

pub struct MujocoHumanoidEnv {
    pub(crate) env: MjEnv,
    pub(crate) config: HumanoidConfig,
    pub(crate) init_qpos: Vec<f64>,
    pub(crate) init_qvel: Vec<f64>,
    pub(crate) rng: StdRng,
    pub(crate) step_count: usize,
}

impl MujocoHumanoidEnv {
    pub fn new(config: Option<HumanoidConfig>) -> 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 {
            init_qpos,
            init_qvel,
            config,
            env,
            rng: StdRng::from_os_rng(),
            step_count: 0,
        })
    }

    fn _get_obs(&self) -> Result<DVector<f64>, Error> {
        let qpos = self.env.qpos();
        let qvel = self.env.qvel();

        let skip_pos = if self.config.exclude_current_positions_from_observation {
            2
        } else {
            0
        };
        let position_iter = qpos.iter().skip(skip_pos);

        let velocity_iter = qvel.iter();

        let cinert_iter = self
            .config
            .include_cinert_in_observation
            .then(|| self.env.cinert().iter().skip(1).flatten())
            .into_iter()
            .flatten();

        let cvel_iter = self
            .config
            .include_cvel_in_observation
            .then(|| self.env.cvel().iter().skip(1).flatten())
            .into_iter()
            .flatten();

        let qfrc_iter = self
            .config
            .include_qfrc_actuator_in_observation
            .then(|| self.env.qfrc_actuator().iter().skip(6))
            .into_iter()
            .flatten();

        let cfrc_ext_iter = self
            .config
            .include_cfrc_ext_in_observation
            .then(|| self.env.cfrc_ext().iter().skip(1).flatten())
            .into_iter()
            .flatten();

        let observation: Vec<f64> = position_iter
            .chain(velocity_iter)
            .chain(cinert_iter)
            .chain(cvel_iter)
            .chain(qfrc_iter)
            .chain(cfrc_ext_iter)
            .cloned()
            .collect();

        Ok(DVector::from_vec(observation))
    }

    fn _calculate_reward(
        &self,
        x_velocity: f64,
        action: &DVector<f64>,
    ) -> Result<(f64, HashMap<String, f64>), Error> {
        let forward_reward = self.config.forward_reward_weight * x_velocity;

        let healthy_reward = if self.is_healthy()? {
            self.config.healthy_reward
        } else {
            0.0
        };

        let ctrl_cost = self._control_cost(action)?;
        let contact_cost = self._contact_cost()?;

        let reward = forward_reward + healthy_reward - ctrl_cost - contact_cost;

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

        Ok((reward, info))
    }

    fn _control_cost(&self, _action: &DVector<f64>) -> Result<f64, Error> {
        let sum_sq_ctrl: f64 = self.env.ctrl().iter().map(|&x| x.powi(2)).sum();

        Ok(self.config.ctrl_cost_weight * sum_sq_ctrl)
    }

    fn _contact_cost(&self) -> Result<f64, Error> {
        let sum_sq_force: f64 = self
            .env
            .cfrc_ext()
            .iter()
            .flatten()
            .map(|&f| f.powi(2))
            .sum();

        let cost = self.config.contact_cost_weight * sum_sq_force;
        Ok(cost.clamp(
            self.config.contact_cost_range.0,
            self.config.contact_cost_range.1,
        ))
    }

    pub fn is_healthy(&self) -> Result<bool, Error> {
        let z_pos = self.env.qpos()[2];
        let (min_z, max_z) = self.config.healthy_z_range;
        Ok(z_pos > min_z && z_pos < max_z)
    }

    fn _mass_center(&self) -> Result<Vector2<f64>, Error> {
        let body_mass = self.env.body_mass();
        let xipos = self.env.xipos();

        let (mass_x, mass_y, total_mass) = body_mass
            .iter()
            .zip(xipos.iter())
            .fold((0.0, 0.0, 0.0), |acc, (&mass, &pos)| {
                (acc.0 + mass * pos[0], acc.1 + mass * pos[1], acc.2 + mass)
            });

        if total_mass > 0.0 {
            Ok(Vector2::new(mass_x / total_mass, mass_y / total_mass))
        } else {
            Ok(Vector2::new(0.0, 0.0))
        }
    }
}

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

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

    fn reset(&mut self, seed: Option<u64>) -> Result<(Self::State, Self::Info), Error> {
        if let Some(s) = seed {
            self.rng = StdRng::seed_from_u64(s);
        }

        self.env.reset_to_initial()?;
        self.step_count = 0;

        let noise_low = -self.config.reset_noise_scale;
        let noise_high = self.config.reset_noise_scale;

        let qpos: Vec<f64> = self
            .init_qpos
            .iter()
            .map(|&x| x + self.rng.random_range(noise_low..noise_high))
            .collect();

        let qvel: Vec<f64> = self
            .init_qvel
            .iter()
            .map(|&x| x + self.rng.random_range(noise_low..noise_high))
            .collect();

        self.env.set_state(&qpos, &qvel)?;

        let obs = self._get_obs()?;

        let mut info = HashMap::new();
        info.insert("z_position".to_string(), self.env.qpos()[2]);

        Ok((obs, info))
    }

    fn step(
        &mut self,
        action: Self::Action,
    ) -> Result<Experience<Self::State, Self::Info, Self::Action>, Error> {
        let curr_state = self.state()?;
        let xy_pos_before = self._mass_center()?;

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

        let xy_pos_after = self._mass_center()?;
        let dt = self.env.dt();
        let xy_velocity = (xy_pos_after - xy_pos_before) / dt;
        let x_velocity = xy_velocity.x;

        let next_state = self._get_obs()?;
        let (reward, reward_info) = self._calculate_reward(x_velocity, &action)?;

        let is_healthy = self.is_healthy()?;
        let terminated = self.config.terminate_when_unhealthy && !is_healthy;

        let truncated = self.step_count >= self.config.max_episode_steps;

        let mut info = reward_info;
        info.insert("x_position".to_string(), self.env.qpos()[0]);
        info.insert("y_position".to_string(), self.env.qpos()[1]);
        info.insert("z_position".to_string(), self.env.qpos()[2]);
        info.insert("x_velocity".to_string(), x_velocity);
        info.insert("y_velocity".to_string(), xy_velocity.y);

        let ten_len_sum: f64 = self.env.ten_length().iter().sum();
        let ten_vel_sum: f64 = self.env.ten_velocity().iter().sum();
        info.insert("tendon_length_sum".to_string(), ten_len_sum);
        info.insert("tendon_velocity_sum".to_string(), ten_vel_sum);

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

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

    fn is_terminal(&self) -> Result<bool, Error> {
        let unhealthy = !self.is_healthy()?;
        Ok(self.config.terminate_when_unhealthy && unhealthy)
    }

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

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

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

    #[test]
    fn test_sanity_initialization() {
        let config = HumanoidConfig::default().with_frame_skip(2);
        let env_res = MujocoHumanoidEnv::new(Some(config));
        assert!(
            env_res.is_ok(),
            "Environment should initialize with valid config"
        );

        let env = env_res.unwrap();
        assert_eq!(env.config.frame_skip, 2, "Config settings should persist");
        assert!(!env.init_qpos.is_empty(), "Initial QPOS should be loaded");
    }

    #[test]
    fn test_reset_noise_functional() {
        let config = HumanoidConfig::default().with_reset_noise_scale(1.0);
        let mut env = MujocoHumanoidEnv::new(Some(config)).unwrap();

        let (_, _) = env.reset(Some(1)).unwrap();
        let start_pos_1 = env.env.qpos().to_vec();

        let (_, _) = env.reset(Some(2)).unwrap();
        let start_pos_2 = env.env.qpos().to_vec();

        assert_ne!(
            start_pos_1, start_pos_2,
            "Different seeds should produce different noise"
        );

        let (_, _) = env.reset(Some(1)).unwrap();
        let start_pos_3 = env.env.qpos().to_vec();

        let diff: f64 = start_pos_1
            .iter()
            .zip(start_pos_3.iter())
            .map(|(a, b)| (a - b).abs())
            .sum();

        assert!(
            diff < 1e-9,
            "Same seed should produce identical starting state"
        );
    }

    #[test]
    fn test_observation_dimensions() {
        let config =
            HumanoidConfig::default().with_observation_settings(true, true, true, true, true);

        let mut env = MujocoHumanoidEnv::new(Some(config)).unwrap();
        let (obs, _) = env.reset(None).unwrap();

        assert!(!obs.is_empty(), "Observation vector should not be empty");
    }

    #[test]
    fn test_step_cycle_and_physics() {
        let mut env = MujocoHumanoidEnv::new(None).unwrap();
        env.reset(None).unwrap();

        let action_dim = env.env.nu();
        let action = DVector::from_element(action_dim, 0.5);

        let step_res = env.step(action);
        assert!(step_res.is_ok());

        let exp = step_res.unwrap();

        assert!(env.env.time() > 0.0);
        assert_eq!(env.step_count, 1);

        assert!(exp.reward.is_finite());

        assert!(!exp.terminal.is_terminated());
    }

    #[test]
    fn test_truncation() {
        let config = HumanoidConfig::default().with_max_episode_steps(2);
        let mut env = MujocoHumanoidEnv::new(Some(config)).unwrap();
        env.reset(None).unwrap();

        let action = DVector::zeros(env.env.nu());

        let exp1 = env.step(action.clone()).unwrap();
        assert!(!exp1.terminal.is_truncated());

        let exp2 = env.step(action).unwrap();
        assert!(
            exp2.terminal.is_truncated(),
            "Environment should truncate at max steps"
        );
    }

    #[test]
    fn test_functional_center_of_mass() {
        let env = MujocoHumanoidEnv::new(None).unwrap();
        let com_res = env._mass_center();
        assert!(com_res.is_ok());
        let com = com_res.unwrap();

        assert!(com.x.abs() < 1.0);
        assert!(com.y.abs() < 1.0);
    }
}