use super::config::{CartPoleConfig, KinematicsIntegrator};
use crate::{
env::environment::{Environment, Error, Experience},
spaces::{Boxed, EnvSpace, Mixed, MixedItem, Space},
};
use nalgebra::SVector;
const FOUR_THIRDS: f64 = 4.0 / 3.0;
const STATE_SIZE: usize = 4;
const ACTION_SIZE: usize = 1;
type Action = MixedItem<ACTION_SIZE>;
type ActionSpace = Mixed<ACTION_SIZE>;
type State = SVector<f64, STATE_SIZE>;
type StateSpace = Boxed<STATE_SIZE>;
#[derive(Debug)]
pub struct CartPole {
t: u32,
config: CartPoleConfig,
state: Option<State>,
pub space: EnvSpace<StateSpace, ActionSpace>,
}
impl CartPole {
pub fn new(config: CartPoleConfig) -> Result<Self, Error> {
let high: State = SVector::from_vec(vec![
config.x_max * 2.0,
f64::MAX,
config.theta_max * 2.0,
f64::MAX,
]);
let space = EnvSpace {
state: Boxed::new(-high, high)?,
action: match config.continuous {
false => Mixed::discrete(2)?,
true => Mixed::continuous(SVector::from_element(-1.0), SVector::from_element(1.0))?,
},
};
Ok(CartPole {
space,
config,
t: 0,
state: None,
})
}
fn compute_acceleration(&self, state: &State, force: &f64) -> (f64, f64) {
let theta: f64 = state[2];
let omega: f64 = state[3];
let (sin_theta, cos_theta) = theta.sin_cos();
let m: f64 = self.config.mc + self.config.mp;
let mpl: f64 = self.config.mp * self.config.l;
let t: f64 = (force + mpl * omega.powi(2) * sin_theta) / m;
let num: f64 = self.config.g * sin_theta - cos_theta * t;
let den: f64 = self.config.l * (FOUR_THIRDS - self.config.mp * cos_theta.powi(2) / m);
let theta_acc: f64 = num / den;
let x_acc: f64 = t - mpl * theta_acc * cos_theta / m;
(x_acc, theta_acc)
}
fn euler(&self, state: &State, x_acc: &f64, theta_acc: &f64) -> State {
let x = state[0];
let v = state[1];
let theta = state[2];
let omega = state[3];
SVector::from_vec(vec![
x + self.config.tau * v,
v + self.config.tau * x_acc,
theta + self.config.tau * omega,
omega + self.config.tau * theta_acc,
])
}
fn semi_implicit_euler(&self, state: &State, x_acc: &f64, theta_acc: &f64) -> State {
let x = state[0];
let v = state[1] + self.config.tau * x_acc;
let theta = state[2];
let omega = state[3] + self.config.tau * theta_acc;
SVector::from_vec(vec![
x + self.config.tau * v,
v,
theta + self.config.tau * omega,
omega,
])
}
fn integrate(&self, state: &State, force: &f64) -> State {
let (x_acc, theta_acc) = self.compute_acceleration(state, force);
match self.config.integrator {
KinematicsIntegrator::Euler => self.euler(state, &x_acc, &theta_acc),
KinematicsIntegrator::SemiImplicitEuler => {
self.semi_implicit_euler(state, &x_acc, &theta_acc)
}
}
}
fn force(&self, action: &Action) -> Result<f64, Error> {
match (&self.space.action, action) {
(Mixed::Discrete(space), Action::Discrete(act)) => {
space.contains(act).map_err(|_| Error::InvalidAction)?;
Ok((2.0 * *act as f64 - 1.0) * self.config.f)
}
(Mixed::Continuous(space), Action::Continuous(act)) => {
space.contains(act).map_err(|_| Error::InvalidAction)?;
Ok(act[0] * self.config.f)
}
_ => Err(Error::InvalidAction),
}
}
}
impl Environment for CartPole {
type Action = Action;
type State = State;
type Info = ();
fn reset(&mut self, seed: Option<u64>) -> Result<(Self::State, Self::Info), Error> {
let state = self.space.state.uniform(seed, -5e-2, 5e-2)?;
self.t = 0;
self.state = Some(state);
Ok((state, ()))
}
fn step(
&mut self,
action: Self::Action,
) -> Result<Experience<Self::State, Self::Info, self::Action>, Error> {
if self.is_done()? {
return Err(Error::EpisodeDone);
}
let curr_state = self.state()?;
let force: f64 = self.force(&action)?;
let next_state = self.integrate(&curr_state, &force);
self.state = Some(next_state);
self.t += 1;
Ok(Experience::new(
curr_state,
1.,
action,
next_state,
(),
self.to_terminal()?,
self.t,
))
}
fn state(&self) -> Result<Self::State, Error> {
match self.state {
Some(s) => Ok(s),
None => Err(Error::NotInitialized),
}
}
fn is_terminal(&self) -> Result<bool, Error> {
let state = self.state()?;
Ok(state[0].abs() > self.config.x_max || state[2].abs() > self.config.theta_max)
}
fn is_truncated(&self) -> bool {
self.t >= self.config.t_max
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::env::environment::Error;
use crate::spaces::{Mixed, MixedItem};
use nalgebra::SVector;
fn default_config() -> CartPoleConfig {
CartPoleConfig {
mc: 1.0,
mp: 0.1,
l: 0.5,
f: 10.0,
g: 9.8,
tau: 0.02,
x_max: 2.4,
theta_max: 0.21,
t_max: 200,
integrator: KinematicsIntegrator::Euler,
continuous: false,
}
}
#[test]
fn test_new_discrete_and_continuous() {
let cfg_discrete = default_config();
let env_d = CartPole::new(cfg_discrete.clone()).unwrap();
match env_d.space.action {
Mixed::Discrete(_) => {}
_ => panic!("Expected discrete action space"),
}
let mut cfg_cont = cfg_discrete.clone();
cfg_cont.continuous = true;
let env_c = CartPole::new(cfg_cont.clone()).unwrap();
match env_c.space.action {
Mixed::Continuous(_) => {}
_ => panic!("Expected continuous action space"),
}
}
#[test]
fn test_compute_acceleration_values() {
let cfg = default_config();
let env = CartPole::new(cfg).unwrap();
let state = SVector::<f64, STATE_SIZE>::from_vec(vec![0.0, 0.0, 0.1, 0.0]);
let (xa, ta) = env.compute_acceleration(&state, &1.0);
assert!(xa.is_finite());
assert!(ta.is_finite());
}
#[test]
fn test_euler_and_semi_implicit() {
let cfg = default_config();
let env = CartPole::new(cfg).unwrap();
let state = SVector::<f64, STATE_SIZE>::from_element(0.0);
let updated_euler = env.euler(&state, &1.0, &-1.0);
assert_eq!(updated_euler.len(), STATE_SIZE);
let updated_semi = env.semi_implicit_euler(&state, &1.0, &-1.0);
assert_eq!(updated_semi.len(), STATE_SIZE);
}
#[test]
fn test_integrate_switches() {
let mut cfg = default_config();
cfg.integrator = KinematicsIntegrator::Euler;
let env_e = CartPole::new(cfg.clone()).unwrap();
let state = SVector::<f64, STATE_SIZE>::from_vec(vec![0.0, 0.0, 0.1, 0.0]);
let res_e = env_e.integrate(&state, &1.0);
assert_eq!(res_e.len(), STATE_SIZE);
cfg.integrator = KinematicsIntegrator::SemiImplicitEuler;
let env_s = CartPole::new(cfg).unwrap();
let res_s = env_s.integrate(&state, &1.0);
assert_eq!(res_s.len(), STATE_SIZE);
}
#[test]
fn test_force_discrete_and_continuous() {
let cfg_d = default_config();
let env_d = CartPole::new(cfg_d).unwrap();
assert_eq!(
env_d.force(&MixedItem::Discrete(0)).unwrap(),
-env_d.config.f
);
assert_eq!(
env_d.force(&MixedItem::Discrete(1)).unwrap(),
env_d.config.f
);
let mut cfg_c = default_config();
cfg_c.continuous = true;
let env_c = CartPole::new(cfg_c).unwrap();
let val = env_c
.force(&MixedItem::Continuous(SVector::from_element(0.5)))
.unwrap();
assert!((val - env_c.config.f * 0.5).abs() < 1e-12);
}
#[test]
fn test_reset_initializes_state() {
let cfg = default_config();
let mut env = CartPole::new(cfg).unwrap();
let (state, _) = env.reset(Some(42)).unwrap();
assert_eq!(state.len(), STATE_SIZE);
assert_eq!(env.t, 0);
assert!(env.state.is_some());
}
#[test]
fn test_step_advances_and_returns_experience() {
let cfg = default_config();
let mut env = CartPole::new(cfg).unwrap();
env.reset(Some(123)).unwrap();
let action = MixedItem::Discrete(1);
let exp = env.step(action.clone()).unwrap();
assert_eq!(exp.step, 1);
assert_eq!(exp.action, action);
assert_eq!(env.t, 1);
assert!(env.state.is_some());
}
#[test]
fn test_step_errors_if_done() {
let mut cfg = default_config();
cfg.x_max = 0.1;
let mut env = CartPole::new(cfg).unwrap();
env.reset(Some(1)).unwrap();
{
let mut s = env.state.unwrap();
s[0] = 1.0;
env.state = Some(s);
}
assert!(matches!(
env.step(MixedItem::Discrete(0)),
Err(Error::EpisodeDone)
));
}
#[test]
fn test_state_and_not_initialized() {
let cfg = default_config();
let env = CartPole::new(cfg).unwrap();
assert!(matches!(env.state(), Err(Error::NotInitialized)));
}
#[test]
fn test_is_terminal_true_and_false() {
let cfg = default_config();
let mut env = CartPole::new(cfg.clone()).unwrap();
env.reset(Some(1)).unwrap();
{
let mut s = env.state.unwrap();
s[0] = cfg.x_max + 0.1;
env.state = Some(s);
}
assert!(env.is_terminal().unwrap());
env.reset(Some(2)).unwrap();
assert!(!env.is_terminal().unwrap());
}
#[test]
fn test_is_truncated() {
let cfg = default_config();
let mut env = CartPole::new(cfg.clone()).unwrap();
env.t = cfg.t_max;
assert!(env.is_truncated());
env.t = cfg.t_max - 1;
assert!(!env.is_truncated());
}
}