time_discretization.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_TIME_DISCRETIZATION_HPP_ 
#define ROBOTOC_TIME_DISCRETIZATION_HPP_
 
#include <vector>
#include <memory>
#include <limits>
#include <cmath>
#include <iostream>
 
#include "robotoc/planner/contact_sequence.hpp"
#include "robotoc/ocp/grid_info.hpp"
 
 
namespace robotoc {
 
class TimeDiscretization {
public:
  TimeDiscretization(const double T, const int N, 
                     const int reserved_num_discrete_events=0);
 
  TimeDiscretization();
 
  ~TimeDiscretization() = default;
 
  TimeDiscretization(const TimeDiscretization&) = default;
 
  TimeDiscretization& operator=(const TimeDiscretization&) = default;
 
  TimeDiscretization(TimeDiscretization&&) noexcept = default;
 
  TimeDiscretization& operator=(TimeDiscretization&&) noexcept = default;
 
  int N() const {
    return N_;
  }
 
  inline int size() const {
    return num_grids_ + 1;
  }
 
  inline const GridInfo& grid(const int i) const {
    assert(i >= 0);
    assert(i < size());
    return grid_[i];
  }
 
  inline const GridInfo& operator[] (const int i) const {
    assert(i >= 0);
    assert(i < size());
    return grid_[i];
  }
 
  inline const GridInfo& front() const {
    return grid_[0];
  }
 
  inline const GridInfo& back() const {
    return grid_[size()-1];
  }
 
  double maxTimeStep() const {
    double max_dt = grid_[0].dt;
    for (int i=0; i<num_grids_; ++i) {
      max_dt = std::max(grid_[i].dt, max_dt);
    }
    return max_dt;
  }
 
  void reserve(const int reserved_num_discrete_events);
 
  int reservedNumDiscreteEvents() const;
 
  void discretize(const std::shared_ptr<ContactSequence>& contact_sequence, const double t);
 
  void correctTimeSteps(const std::shared_ptr<ContactSequence>& contact_sequence, const double t);
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, 
                                  const TimeDiscretization& discretization);
 
private:
  double T_, max_dt_, eps_;
  int N_, num_grids_, reserved_num_discrete_events_;
  std::vector<GridInfo> grid_;
  std::vector<bool> sto_event_, sto_phase_;
};
 
} // namespace robotoc
 
#endif // ROBOTOC_TIME_DISCRETIZATION_HPP_