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/humanoid.xml");
const ACTION_MINIMUM: f64 = -0.4;
const ACTION_MAXIMUM: f64 = 0.4;
#[derive(Debug, Clone, PartialEq)]
pub struct HumanoidV5Info {
pub x_position: f32,
pub y_position: f32,
pub tendon_length: Vec<f32>,
pub tendon_velocity: Vec<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>,
}
pub struct HumanoidV5 {
core: MujocoCore,
forward_reward_weight: f64,
control_cost_weight: f64,
contact_cost_weight: f64,
contact_cost_range: (f64, f64),
healthy_reward: f64,
terminate_when_unhealthy: bool,
healthy_z_range: (f64, f64),
reset_noise_scale: f64,
exclude_xy: bool,
include_cinert: bool,
include_cvel: bool,
include_qfrc_actuator: bool,
include_cfrc_ext: bool,
}
#[bon]
impl HumanoidV5 {
#[builder]
pub fn new(
#[builder(default = &Device::Cpu)] device: &Device,
#[builder(default = 5)] frame_skip: usize,
#[builder(default = 1.25)] forward_reward_weight: f64,
#[builder(default = 0.1)] ctrl_cost_weight: f64,
#[builder(default = 5e-7)] contact_cost_weight: f64,
#[builder(default = (f64::NEG_INFINITY, 10.0))] contact_cost_range: (f64, f64),
#[builder(default = 5.0)] healthy_reward: f64,
#[builder(default = true)] terminate_when_unhealthy: bool,
#[builder(default = (1.0, 2.0))] healthy_z_range: (f64, f64),
#[builder(default = 1e-2)] reset_noise_scale: f64,
#[builder(default = true)] exclude_current_positions_from_observation: bool,
#[builder(default = true)] include_cinert_in_observation: bool,
#[builder(default = true)] include_cvel_in_observation: bool,
#[builder(default = true)] include_qfrc_actuator_in_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("contact_cost_range", contact_cost_range)?;
validate_range("healthy_z_range", healthy_z_range)?;
Ok(Self {
core: MujocoCore::new(MODEL, frame_skip, device, render)?,
forward_reward_weight,
control_cost_weight: ctrl_cost_weight,
contact_cost_weight,
contact_cost_range,
healthy_reward,
terminate_when_unhealthy,
healthy_z_range,
reset_noise_scale,
exclude_xy: exclude_current_positions_from_observation,
include_cinert: include_cinert_in_observation,
include_cvel: include_cvel_in_observation,
include_qfrc_actuator: include_qfrc_actuator_in_observation,
include_cfrc_ext: include_cfrc_ext_in_observation,
})
}
pub fn seed(&mut self, seed: u64) {
self.core.seed(seed);
}
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.healthy_z_range.0 < z && z < self.healthy_z_range.1
}
fn contact_cost(&self) -> f64 {
let cost = self.contact_cost_weight
* self
.core
.cfrc_ext()
.iter()
.flatten()
.map(|force| force * force)
.sum::<f64>();
cost.clamp(self.contact_cost_range.0, self.contact_cost_range.1)
}
fn observation_size(&self) -> usize {
let positions = self.core.nq() - if self.exclude_xy { 2 } else { 0 };
positions
+ self.core.nv()
+ usize::from(self.include_cinert) * (self.core.nbody() - 1) * 10
+ usize::from(self.include_cvel) * (self.core.nbody() - 1) * 6
+ usize::from(self.include_qfrc_actuator) * (self.core.nv() - 6)
+ usize::from(self.include_cfrc_ext) * (self.core.nbody() - 1) * 6
}
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_cinert {
observation.extend(self.core.cinert()[1..].iter().flatten().copied());
}
if self.include_cvel {
observation.extend(self.core.cvel()[1..].iter().flatten().copied());
}
if self.include_qfrc_actuator {
observation.extend_from_slice(&self.core.qfrc_actuator()[6..]);
}
if self.include_cfrc_ext {
observation.extend(self.core.cfrc_ext()[1..].iter().flatten().copied());
}
self.core.tensor(&observation)
}
fn info(&self) -> HumanoidV5Info {
let x = self.core.qpos()[0];
let y = self.core.qpos()[1];
HumanoidV5Info {
x_position: x as f32,
y_position: y as f32,
tendon_length: self
.core
.tendon_lengths()
.iter()
.map(|value| *value as f32)
.collect(),
tendon_velocity: self
.core
.tendon_velocities()
.iter()
.map(|value| *value as f32)
.collect(),
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<HumanoidV5Info> for HumanoidV5 {
type Error = MujocoError;
type SpaceError = candle_core::Error;
fn reset(&mut self) -> Result<ResetInfo<HumanoidV5Info>, Self::Error> {
self.core.reset_uniform(self.reset_noise_scale)?;
self.core.render()?;
Ok(ResetInfo {
state: self.observation()?,
info: self.info(),
})
}
fn step(&mut self, action: Tensor) -> Result<StepInfo<HumanoidV5Info>, Self::Error> {
let position_before = self.core.mass_center_xy();
let action = self.core.step(&action)?;
let position_after = self.core.mass_center_xy();
let x_velocity = (position_after[0] - position_before[0]) / 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(((position_after[1] - position_before[1]) / self.core.dt()) 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_bounded(ACTION_MINIMUM, ACTION_MAXIMUM)
}
}