modurl_mujoco 0.1.0

Gymnasium-compatible MuJoCo environments for ModuRL
Documentation
use bon::bon;
use candle_core::{Device, Tensor};
use modurl::{
    gym::{Gym, ResetInfo, StepInfo},
    spaces::Space,
};

use crate::{
    MujocoError,
    core::{MujocoCore, validate_noise_scale, validate_range},
};

const MODEL: &str = include_str!("../assets/ant.xml");

#[derive(Debug, Clone, PartialEq)]
pub struct AntV5Info {
    pub x_position: f32,
    pub y_position: f32,
    pub distance_from_origin: f32,
    pub x_velocity: Option<f32>,
    pub y_velocity: Option<f32>,
    pub reward_forward: Option<f32>,
    pub reward_ctrl: Option<f32>,
    pub reward_contact: Option<f32>,
    pub reward_survive: Option<f32>,
}

/// Gymnasium-compatible `Ant-v5` with the default model.
pub struct AntV5 {
    core: MujocoCore,
    forward_reward_weight: f64,
    control_cost_weight: f64,
    contact_cost_weight: f64,
    healthy_reward: f64,
    terminate_when_unhealthy: bool,
    healthy_z_range: (f64, f64),
    contact_force_range: (f64, f64),
    reset_noise_scale: f64,
    exclude_xy: bool,
    include_contact_forces: bool,
}

#[bon]
impl AntV5 {
    /// Creates an environment. All parameters default to Gymnasium v5 values.
    #[builder]
    pub fn new(
        #[builder(default = &Device::Cpu)] device: &Device,
        #[builder(default = 5)] frame_skip: usize,
        #[builder(default = 1.0)] forward_reward_weight: f64,
        #[builder(default = 0.5)] ctrl_cost_weight: f64,
        #[builder(default = 5e-4)] contact_cost_weight: f64,
        #[builder(default = 1.0)] healthy_reward: f64,
        #[builder(default = true)] terminate_when_unhealthy: bool,
        #[builder(default = (0.2, 1.0))] healthy_z_range: (f64, f64),
        #[builder(default = (-1.0, 1.0))] contact_force_range: (f64, f64),
        #[builder(default = 0.1)] reset_noise_scale: f64,
        #[builder(default = true)] exclude_current_positions_from_observation: bool,
        #[builder(default = true)] include_cfrc_ext_in_observation: bool,
        #[cfg(feature = "rendering")]
        #[builder(default = false)]
        render: bool,
    ) -> Result<Self, MujocoError> {
        #[cfg(not(feature = "rendering"))]
        let render = false;

        validate_noise_scale(reset_noise_scale)?;
        validate_range("healthy_z_range", healthy_z_range)?;
        validate_range("contact_force_range", contact_force_range)?;
        Ok(Self {
            core: MujocoCore::new(MODEL, frame_skip, device, render)?,
            forward_reward_weight,
            control_cost_weight: ctrl_cost_weight,
            contact_cost_weight,
            healthy_reward,
            terminate_when_unhealthy,
            healthy_z_range,
            contact_force_range,
            reset_noise_scale,
            exclude_xy: exclude_current_positions_from_observation,
            include_contact_forces: include_cfrc_ext_in_observation,
        })
    }

    /// Re-seeds the reset-noise generator.
    pub fn seed(&mut self, seed: u64) {
        self.core.seed(seed);
    }

    /// Sets an exact MuJoCo state, useful for reproducible evaluation and parity tests.
    pub fn set_state(&mut self, qpos: &[f64], qvel: &[f64]) -> Result<Tensor, MujocoError> {
        self.core.set_state(qpos, qvel)?;
        self.core.render()?;
        self.observation()
    }

    fn is_healthy(&self) -> bool {
        let z = self.core.qpos()[2];
        self.core
            .qpos()
            .iter()
            .chain(self.core.qvel())
            .all(|value| value.is_finite())
            && self.healthy_z_range.0 <= z
            && z <= self.healthy_z_range.1
    }

    fn contact_cost(&self) -> f64 {
        let (minimum, maximum) = self.contact_force_range;
        self.contact_cost_weight
            * self
                .core
                .cfrc_ext()
                .iter()
                .flatten()
                .map(|force| force.clamp(minimum, maximum).powi(2))
                .sum::<f64>()
    }

    fn observation_size(&self) -> usize {
        let positions = self.core.nq() - if self.exclude_xy { 2 } else { 0 };
        let contacts = if self.include_contact_forces {
            (self.core.nbody() - 1) * 6
        } else {
            0
        };
        positions + self.core.nv() + contacts
    }

    fn observation(&self) -> Result<Tensor, MujocoError> {
        let mut observation = Vec::with_capacity(self.observation_size());
        observation.extend_from_slice(&self.core.qpos()[if self.exclude_xy { 2 } else { 0 }..]);
        observation.extend_from_slice(self.core.qvel());
        if self.include_contact_forces {
            let (minimum, maximum) = self.contact_force_range;
            observation.extend(
                self.core.cfrc_ext()[1..]
                    .iter()
                    .flatten()
                    .map(|force| force.clamp(minimum, maximum)),
            );
        }
        self.core.tensor(&observation)
    }

    fn info(&self) -> AntV5Info {
        let x = self.core.qpos()[0];
        let y = self.core.qpos()[1];
        AntV5Info {
            x_position: x as f32,
            y_position: y as f32,
            distance_from_origin: x.hypot(y) as f32,
            x_velocity: None,
            y_velocity: None,
            reward_forward: None,
            reward_ctrl: None,
            reward_contact: None,
            reward_survive: None,
        }
    }
}

impl Gym<AntV5Info> for AntV5 {
    type Error = MujocoError;
    type SpaceError = candle_core::Error;

    fn reset(&mut self) -> Result<ResetInfo<AntV5Info>, Self::Error> {
        self.core
            .reset_uniform_positions_normal_velocities(self.reset_noise_scale)?;
        self.core.render()?;
        Ok(ResetInfo {
            state: self.observation()?,
            info: self.info(),
        })
    }

    /// Steps with one continuous actuator vector shaped `[8]`.
    fn step(&mut self, action: Tensor) -> Result<StepInfo<AntV5Info>, Self::Error> {
        let position_before = self.core.body_position(1);
        let action = self.core.step(&action)?;
        let position_after = self.core.body_position(1);
        let x_velocity = (position_after[0] - position_before[0]) / self.core.dt();
        let y_velocity = (position_after[1] - position_before[1]) / self.core.dt();
        let healthy = self.is_healthy();
        let control_cost = self.control_cost_weight * action.iter().map(|x| x * x).sum::<f64>();
        let forward_reward = self.forward_reward_weight * x_velocity;
        let survive_reward = if healthy { self.healthy_reward } else { 0.0 };
        let contact_cost = self.contact_cost();
        let reward = forward_reward + survive_reward - control_cost - contact_cost;
        self.core.render()?;
        let mut info = self.info();
        info.x_velocity = Some(x_velocity as f32);
        info.y_velocity = Some(y_velocity as f32);
        info.reward_forward = Some(forward_reward as f32);
        info.reward_ctrl = Some(-control_cost as f32);
        info.reward_contact = Some(-contact_cost as f32);
        info.reward_survive = Some(survive_reward as f32);
        Ok(StepInfo {
            state: self.observation()?,
            reward: reward as f32,
            done: !healthy && self.terminate_when_unhealthy,
            truncated: false,
            info,
        })
    }

    fn observation_space(&self) -> Box<dyn Space<Error = Self::SpaceError>> {
        self.core
            .unbounded_observation_space(self.observation_size())
    }

    fn action_space(&self) -> Box<dyn Space<Error = Self::SpaceError>> {
        self.core.action_space()
    }
}