Weight.h

Go to the documentation of this file.

#ifndef WEIGHT_H_
#define WEIGHT_H_
 
#include "MPProblem/Robot/Actuator.h"
#include "MPProblem/Robot/Control.h"
#include "MPProblem/Robot/Robot.h"
#include "Geometry/Bodies/MultiBody.h"
#include "Geometry/Boundaries/Range.h"
#include "Utilities/MPUtils.h"
 
#ifdef _PARALLEL
#include "views/proxy.h"
#endif
 
#include "nonstd/numerics.h"
 
#include <iostream>
#include <numeric>
#include <string>
#include <vector>
 
 
template <class CfgType>
class DefaultWeight {
 
  public:
 
 
    DefaultWeight(const std::string& _label = "", const double _w = 0,
        const std::vector<CfgType>& _intermediates = std::vector<CfgType>());
 
    virtual ~DefaultWeight() = default;
 
 
    virtual const DefaultWeight& operator=(const DefaultWeight& _w);
 
 
    virtual bool operator==(const DefaultWeight& _w) const noexcept;
 
    virtual bool operator!=(const DefaultWeight& _w) const noexcept;
 
    virtual bool operator<(const DefaultWeight& _other) const noexcept;
 
 
    const std::string& GetLPLabel() const noexcept;
    void SetLPLabel(const std::string& _lpLabel) noexcept;
 
    std::vector<CfgType>& GetIntermediates() noexcept;
    const std::vector<CfgType>& GetIntermediates() const noexcept;
 
    void SetIntermediates(const std::vector<CfgType>& _intermediates) noexcept;
    void SetIntermediates(std::vector<CfgType>&& _intermediates) noexcept;
 
    double GetWeight() const noexcept;
    void SetWeight(const double _w) noexcept;
 
    void SetControl(const Control& _c) noexcept;
    const ControlSet& GetControlSet() const noexcept;
    ControlSet& GetControlSet() noexcept;
    void SetControlSet(const ControlSet& _c) noexcept;
 
    bool IsChecked(const int _mult) const noexcept;
    void SetChecked(const int _mult) noexcept;
 
    bool HasClearance() const noexcept;
    double GetClearance() const noexcept;
    void SetClearance(const double _c) noexcept;
 
    // The number of timesteps that the local plan on this edge cares about.
    size_t GetTimeSteps() const noexcept;
    void SetTimeSteps(std::size_t _steps) noexcept;
 
 
 
    virtual void Read(std::istream& _is);
 
    virtual void Write(std::ostream& _os) const;
 
    static Robot* inputRobot;
 
    void Clear();
 
 
    virtual DefaultWeight operator+(const DefaultWeight& _other) const ;
 
    double Weight() const noexcept; // For GraphAlgo interface
    static DefaultWeight MaxWeight() noexcept; // For Dijkstra's Alg
 
 
  protected:
 
 
    std::string m_lpLabel;   
 
    double m_weight{0.};                  
    std::vector<CfgType> m_intermediates; 
 
    int m_checkedMult{std::numeric_limits<int>::max()};
 
    double m_clearance{std::numeric_limits<double>::infinity()};
 
    // For nonholonomic robots.
    ControlSet m_controls;   
    size_t m_timeSteps{0};   
 
 
  public:
 
#ifdef _PARALLEL
    void define_type(stapl::typer &t)
    {
      t.member(m_weight);
      t.member(m_lpLabel);
      t.member(m_intermediates);
    }
#endif
};
 
/*------------------------------ Construction --------------------------------*/
 
template <typename CfgType>
DefaultWeight<CfgType>::
DefaultWeight(const std::string& _label, const double _w,
    const std::vector<CfgType>& _intermediates) :
    m_lpLabel(_label), m_weight(_w), m_intermediates(_intermediates) {
}
 
/*------------------------------- Assignment ---------------------------------*/
 
template <typename CfgType>
const DefaultWeight<CfgType>&
DefaultWeight<CfgType>::
operator=(const DefaultWeight<CfgType>& _w) {
  if(this != &_w) {
    m_lpLabel       = _w.GetLPLabel();
    m_weight        = _w.GetWeight();
    m_intermediates = _w.GetIntermediates();
    m_checkedMult   = _w.m_checkedMult;
    m_clearance     = _w.GetClearance();
    m_controls      = _w.GetControlSet();
    m_timeSteps     = _w.GetTimeSteps();
  }
  return *this;
}
 
/*-------------------------- Equality and Ordering ---------------------------*/
 
template <typename CfgType>
bool
DefaultWeight<CfgType>::
operator==(const DefaultWeight& _w) const noexcept {
  static constexpr double tolerance = 100
                                    * std::numeric_limits<double>::epsilon();
 
  return nonstd::approx(m_weight, _w.GetWeight(), tolerance)
     and m_intermediates == _w.GetIntermediates();
}
 
 
template <typename CfgType>
bool
DefaultWeight<CfgType>::
operator!=(const DefaultWeight& _w) const noexcept {
  return !(*this == _w);
}
 
 
template <typename CfgType>
bool
DefaultWeight<CfgType>::
operator<(const DefaultWeight& _w) const noexcept {
  return m_weight < _w.m_weight;
}
 
/*----------------------------------------------------------------------------*/
 
template <typename CfgType>
const std::string&
DefaultWeight<CfgType>::
GetLPLabel() const noexcept {
  return m_lpLabel;
}
 
 
template <typename CfgType>
void
DefaultWeight<CfgType>::
SetLPLabel(const std::string& _label) noexcept {
  m_lpLabel = _label;
}
 
 
template <typename CfgType>
std::vector<CfgType>&
DefaultWeight<CfgType>::
GetIntermediates() noexcept {
  return m_intermediates;
}
 
 
template <typename CfgType>
const std::vector<CfgType>&
DefaultWeight<CfgType>::
GetIntermediates() const noexcept {
  return m_intermediates;
}
 
 
template <typename CfgType>
void
DefaultWeight<CfgType>::
SetIntermediates(const std::vector<CfgType>& _intermediates) noexcept {
  m_intermediates = _intermediates;
}
 
 
template <typename CfgType>
void
DefaultWeight<CfgType>::
SetIntermediates(std::vector<CfgType>&& _intermediates) noexcept {
  m_intermediates = std::move(_intermediates);
}
 
 
template <typename CfgType>
double
DefaultWeight<CfgType>::
GetWeight() const noexcept {
  return m_weight;
}
 
 
template <typename CfgType>
void
DefaultWeight<CfgType>::
SetWeight(const double _w) noexcept {
  m_weight = _w;
}
 
 
//------------------------- Control Set Modifiers ------------------------------
 
//This is a wrapper for SetControlSet(), because right now the only use case is
// single controls:
template <typename CfgType>
void
DefaultWeight<CfgType>::
SetControl(const Control& _c) noexcept {
  SetControlSet({_c});
}
 
template <typename CfgType>
const ControlSet&
DefaultWeight<CfgType>::
GetControlSet() const noexcept {
  return m_controls;
}
 
template <typename CfgType>
ControlSet&
DefaultWeight<CfgType>::
GetControlSet() noexcept {
  return m_controls;
}
 
 
// We can potentially have multiple controls to get from one configuration to
// the next, so the entire set of those controls can be set here.
template <typename CfgType>
void
DefaultWeight<CfgType>::
SetControlSet(const ControlSet& _c) noexcept {
  m_controls = _c;
}
 
 
template <typename CfgType>
std::size_t
DefaultWeight<CfgType>::
GetTimeSteps() const noexcept {
  return m_timeSteps;
}
 
template <typename CfgType>
void
DefaultWeight<CfgType>::
SetTimeSteps(std::size_t _steps) noexcept {
  m_timeSteps = _steps;
}
 
 
template <typename CfgType>
bool
DefaultWeight<CfgType>::
IsChecked(const int _mult) const noexcept {
  return m_checkedMult <= _mult;
}
 
 
template <typename CfgType>
void
DefaultWeight<CfgType>::
SetChecked(const int _mult) noexcept {
  m_checkedMult = std::min(m_checkedMult, _mult);
}
 
 
template <typename CfgType>
double
DefaultWeight<CfgType>::
GetClearance() const noexcept {
  return m_clearance;
}
 
 
template <typename CfgType>
void
DefaultWeight<CfgType>::
SetClearance(const double _c) noexcept {
  m_clearance = _c;
}
 
/*----------------------------------- I/O ------------------------------------*/
 
template <typename CfgType>
void
DefaultWeight<CfgType>::
Clear() {
  // Reset the initial state variables of this object:
  m_lpLabel.clear();
  m_intermediates.clear();
  m_controls.clear();
  m_weight      = 0.;
  m_checkedMult = std::numeric_limits<int>::max();
  m_clearance   = std::numeric_limits<double>::infinity();
  m_timeSteps   = 0;
}
 
// Initialize the input robot (a placeholder for not knowing the robot when
// reading in cfgs from a file) to nullptr, as it is to be set externally.
template <typename CfgType>
Robot* DefaultWeight<CfgType>::inputRobot = nullptr;
 
 
// Reads in an edge from a standard input stream, including
// control signals for nonholonomic robots.
// This function is symmetric with Write().
template <typename CfgType>
void
DefaultWeight<CfgType>::
Read(std::istream& _is) {
  if(!inputRobot)
    throw RunTimeException(WHERE) << "Need to tell the weight class what robot "
                                  << "we are reading for. Use inputRobot "
                                  << "member to specify.";
 
  Clear(); //Necessary, as stapl seems to use the same temp weight object
 
  // Read the data for robots that are either holonomic or not:
  size_t numIntermediates;
  _is >> numIntermediates;
  m_intermediates.clear();
 
  CfgType tmp(inputRobot);
  for(size_t i = 0; i < numIntermediates; ++i) {
    _is >> tmp;
    m_intermediates.push_back(tmp);
  }
  _is >> m_weight;
 
  #ifndef VIZMO_MAP
    // Read the data specifically for nonholonomic robots if necessary:
    if(inputRobot->IsNonholonomic()) {
      //read the # timesteps used for this edge and the # actuators/controls
      _is >> m_timeSteps;
      std::size_t numControls;
      _is >> numControls;
 
      //Now loop through each individual control, outputting the full signal
      // and label for that control:
      for(std::size_t i = 0; i < numControls; i++) {
        Control con;
        std::string label;
        _is >> label;
 
 
        //If the label is "Coast" then we need to put a nullptr for the actuator
        // since that's what is used for a coast control.
        if(label.compare("Coast") == 0) // Note compare() returns 0 if equal.
          con.actuator = nullptr;
        else
          con.actuator = inputRobot->GetActuator(label);
 
        //Loop through and get all of the control signal's values.
        //The number of values in the signal is equal to the robot's DOF.
        std::size_t signalLength = inputRobot->GetMultiBody()->DOF();
        con.signal.resize(signalLength, 0);
        for(std::size_t j = 0; j < signalLength; j++) {
          double val;
          _is >> val;
          con.signal[j] = val;
        }
 
        //It wouldn't be a bad idea to put in a check here to ensure that if
        // the actuator is a nullptr, then all of the signal is 0.
 
        //finally push back the read-in control signal:
        m_controls.push_back(con);
      }
    }
  #endif
}
 
 
// Writes to a standard output stream all of the data for the edge, including
// control signals for nonholonomic robots.
// This function is symmetric with Write().
template <typename CfgType>
void
DefaultWeight<CfgType>::
Write(std::ostream& _os) const {
 
  //Write the data that's needed whether it's a holonomic robot or not:
  _os << m_intermediates.size() << " ";
  for(auto&  cfg : m_intermediates)
    _os << cfg;
  _os << std::scientific << std::setprecision(16) << m_weight;
 
  #ifndef VIZMO_MAP
    //If nonholonomic, print extra data needed (holonomic will have no controls)
    if(!m_controls.empty()) {
      //output the # timesteps used for this edge and the # actuators/controls
      _os << " " << m_timeSteps << " " << m_controls.size() << " ";
 
      //Now loop through each individual control, outputting the full signal
      // and label for that control:
      for(auto control : m_controls) {
        //Output the actuator label:
        if(control.actuator)
          _os << control.actuator->GetLabel() << " ";
        else
          _os << "Coast ";// if actuator is nullptr, it's a coast control.
 
        //Write each of the control signal's values:
        for (double& val : control.signal)
          _os << val << " ";
      }
    }
  #endif
 
  // Clear scientific/precision options.
  _os.unsetf(std::ios_base::floatfield);
}
 
 
 
template <typename CfgType>
std::ostream&
operator<<(std::ostream& _os, const DefaultWeight<CfgType>& _w) {
  _w.Write(_os);
  return _os;
}
 
 
template <typename CfgType>
std::istream&
operator>>(std::istream& _is, DefaultWeight<CfgType>& _w) {
  _w.Read(_is);
  return _is;
}
 
/*---------------------- stapl graph interface helpers -----------------------*/
 
template <typename CfgType>
DefaultWeight<CfgType>
DefaultWeight<CfgType>::
operator+(const DefaultWeight& _other) const {
  return DefaultWeight(m_lpLabel, m_weight + _other.m_weight);
}
 
 
template <typename CfgType>
double
DefaultWeight<CfgType>::
Weight() const noexcept {
  return GetWeight();
}
 
 
template <typename CfgType>
DefaultWeight<CfgType>
DefaultWeight<CfgType>::
MaxWeight() noexcept {
  static constexpr double max = std::numeric_limits<double>::max();
  return DefaultWeight("INVALID", max);
}
 
/*----------------------------------------------------------------------------*/
 
#endif