path-planning 0.1.0

/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */

use std::marker::PhantomData;

use nalgebra::SVector;
use num_traits::Zero;
use serde::{de::DeserializeOwned, Deserialize, Serialize};

use crate::cspace::CSpace;
use crate::error::Result;
use crate::obstacles::ObstacleSpace;
use crate::params::FromParams;
use crate::run::Runnable;
use crate::scalar::Scalar;
use crate::trajectories::{FullTrajOwned, FullTrajRefOwned, FullTrajectory};

/// A local goal location, specifies whether this location is new or if it is the same as before.
#[derive(Clone, Copy, Debug)]
pub enum MoveGoal<X, const N: usize> {
  /// The current move goal has changed
  New(SVector<X, N>),
  /// The current move goal has not changed
  Old(SVector<X, N>),
  /// The global goal has been reached
  Finished,
}

/// Information about the robot sizing
#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize,",
  deserialize = "X: DeserializeOwned,",
))]
pub struct RobotSpecs<X: Scalar> {
  /// The radius of the bounding sphere around the robot
  pub robot_radius: X,
  /// The radius around the robot that is able to sense obstacles
  pub sensor_radius: X,
}

/// A sampling-based path path_planner
pub trait PathPlanner<X, CS, OS, const N: usize>: Sized
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
{
  type Params: Clone;
  type State: Clone;

  /// Define a new Path Planner with an initial set of obstacles in the space.
  ///
  /// `init`: The inital pose, must be in free space.
  ///
  /// `goal`: The goal pose, must be in free space, the root of the search tree.
  ///
  /// `robot_specs`: Information about the robot sizing.
  ///
  /// `cspace`: Describes the constant configuration space to plan within.
  ///
  /// `obs_space`: The obstacle space to plan around.
  ///
  /// `params`: Algorithm specific parameters.
  fn new(
    init: SVector<X, N>,
    goal: SVector<X, N>,
    robot_specs: RobotSpecs<X>,
    cspace: CS,
    obs_space: OS,
    params: Self::Params,
  ) -> Result<Self>;

  /// Samples nodes until one is added to the search graph.
  ///
  /// Returns updated state reference once node is successfully added to the graph
  fn create_node(&mut self) -> &Self::State;

  /// Sample once to add a node to the graph
  ///
  /// Optionally returns updated state if node was successfully added
  fn sample_node(&mut self) -> Option<&Self::State>;

  /// Checks the sensor on the robot for obstacles updates within robot_specs.sensor_range
  fn check_sensors(&mut self);

  /// Returns reference to the currently used obstacle space
  fn get_obs_space(&self) -> &OS;

  /// Returns mutable reference to the currently used obstacle space
  ///
  /// Make sure to call [`Self::check_nodes`] with `obs_space = true` if the modifications
  /// might make some nodes invalid
  fn get_obs_space_mut(&mut self) -> &mut OS;

  /// Returns reference to the currently used cspace
  fn get_cspace(&self) -> &CS;

  /// Returns mutable reference to the currently used cspace
  ///
  /// Make sure to call [`Self::check_nodes`] with `cspace = true` if the modifications
  /// might make some nodes invalid
  fn get_cspace_mut(&mut self) -> &mut CS;

  /// Checks the spaces specified that all nodes are still valid in that space
  ///
  /// Make sure to call after modifying the obs_space or cspace such that some
  /// nodes might be invalid
  fn check_nodes(&mut self, obs_space: bool, cspace: bool);

  /// Updates the pose of the system, possibly changing the move goal
  ///
  /// Returns Some coordinates the next move goal if the global goal is reachable
  ///
  /// if `nearest` is true, will select a move goal that is the closest to the given pose
  ///
  /// Specifies wether the returned move goal is new or the same as before
  fn update_pose(
    &mut self,
    pose: SVector<X, N>,
    nearest: bool,
  ) -> Option<MoveGoal<X, N>>;

  /// Get the search tree as a list of edges
  fn get_tree(&self) -> Vec<FullTrajRefOwned<X, CS::Traj, N>>;

  /// Returns Some trajectory the next move goal if the global goal is reachable
  fn get_current_path(&self) -> Option<FullTrajOwned<X, CS::Traj, N>>;

  /// Returns Some path to the goal if the global goal is reachable
  fn get_path_to_goal(&self) -> Option<Vec<SVector<X, N>>>;

  /// Returns the total cost to reach the goal from the most recent pose given in update_pose
  fn get_cost_to_goal(&self) -> Option<X> {
    let iter = self.get_path_to_goal()?;

    let mut cost = X::zero();
    let mut last_pose = self.get_last_pose().clone();

    for pose in iter {
      cost += self.get_cspace().cost(&last_pose, &pose);
      last_pose = pose;
    }

    Some(cost)
  }

  fn get_cost_to_move_goal(&self) -> Option<X> {
    let current_path = self.get_current_path()?;
    let move_goal = current_path.end();
    let pose = self.get_last_pose().clone();

    Some(self.get_cspace().cost(&pose, &move_goal))
  }

  /// Get the last known pose of the system
  fn get_last_pose(&self) -> &SVector<X, N>;

  /// Get a reference to the current serializable state information
  fn get_state(&self) -> &Self::State;

  /// Returns the coordinates of the goal node
  fn get_goal(&self) -> &SVector<X, N>;

  /// Returns the number of nodes in the graph plus the initial node (which is technically never added to the graph)
  fn count(&self) -> usize;
}

#[derive(Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize, PP::Params: Serialize, <CS as FromParams>::Params: Serialize, <OS as FromParams>::Params: Serialize",
  deserialize = "X: DeserializeOwned, PP::Params: DeserializeOwned, <CS as FromParams>::Params: DeserializeOwned, <OS as FromParams>::Params: DeserializeOwned",
))]
pub struct PathPlannerParams<X, CS, OS, PP, const N: usize>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
  PP: PathPlanner<X, CS, OS, N>,
{
  pub init: SVector<X, N>,
  pub goal: SVector<X, N>,
  pub robot_specs: RobotSpecs<X>,
  pub nodes_per_step: usize,
  pub max_translation_per_step: X,
  pub planner_params: PP::Params,
  pub cspace: <CS as FromParams>::Params,
  pub obstacles: <OS as FromParams>::Params,
}

impl<X, CS, OS, PP, const N: usize> Clone
  for PathPlannerParams<X, CS, OS, PP, N>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
  PP: PathPlanner<X, CS, OS, N>,
{
  fn clone(&self) -> Self {
    Self {
      init: self.init.clone(),
      goal: self.goal.clone(),
      robot_specs: self.robot_specs.clone(),
      nodes_per_step: self.nodes_per_step.clone(),
      max_translation_per_step: self.max_translation_per_step.clone(),
      planner_params: self.planner_params.clone(),
      cspace: self.cspace.clone(),
      obstacles: self.obstacles.clone(),
    }
  }
}

/// A generic implementation of [`Runnable`] for implementor for [`PathPlanner`]
pub struct RunPathPlanner<X, CS, OS, PP, const N: usize>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
  PP: PathPlanner<X, CS, OS, N>,
{
  path_planner: PP,
  nodes_per_step: usize,
  max_translation_per_step: X,
  phantom_data: PhantomData<(CS, OS)>,
}

impl<X, CS, OS, PP, const N: usize> FromParams
  for RunPathPlanner<X, CS, OS, PP, N>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
  PP: PathPlanner<X, CS, OS, N>,
{
  type Params = PathPlannerParams<X, CS, OS, PP, N>;

  fn from_params(params: Self::Params) -> Result<Self> {
    let path_planner = PP::new(
      params.init,
      params.goal,
      params.robot_specs,
      CS::from_params(params.cspace)?,
      OS::from_params(params.obstacles)?,
      params.planner_params,
    )?;

    Ok(Self {
      path_planner,
      nodes_per_step: params.nodes_per_step,
      max_translation_per_step: params.max_translation_per_step,
      phantom_data: PhantomData,
    })
  }
}

impl<X, CS, OS, PP, const N: usize> Runnable
  for RunPathPlanner<X, CS, OS, PP, N>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
  PP: PathPlanner<X, CS, OS, N>,
  PP::Params: Clone,
  PP::State: Clone,
{
  type State = PP::State;

  /// TODO: Currently assumes that obstacles are static. Appearing obstacles
  /// should be dealt with here
  fn step(&mut self) -> Option<&Self::State> {
    // Sample new nodes
    let begin_count = self.path_planner.count();
    for _ in 0..self.nodes_per_step {
      self.path_planner.create_node();
      log::debug!("Added node {} to tree", self.path_planner.count());
    }
    log::info!(
      "Added nodes {}-{} to the tree",
      begin_count,
      self.path_planner.count()
    );

    // Update pose to same location to check if any of the newly added node will
    // give us a valid path
    let pose = self.path_planner.get_last_pose().clone();
    if let Some(MoveGoal::Finished) = self.path_planner.update_pose(pose, false)
    {
      return None;
    }

    if let Some(traj) = self.path_planner.get_current_path() {
      let target = traj.end();
      let cspace = self.path_planner.get_cspace();
      let cost = cspace.cost(&pose, target);

      let new_pose = if cost >= self.max_translation_per_step {
        let direction = *target - pose;
        let unit_direction =
          direction / cspace.cost(&SVector::zero(), &direction);
        let direction = unit_direction * self.max_translation_per_step;
        let new_pose = pose + direction;
        new_pose
      } else {
        target.clone()
      };

      if let Some(MoveGoal::Finished) =
        self.path_planner.update_pose(new_pose, false)
      {
        return None;
      }
    }
    Some(self.path_planner.get_state())
  }
}