Cfg.h

Go to the documentation of this file.

#ifndef PMPL_CFG_H_
#define PMPL_CFG_H_
 
#include <cstddef>
#include <iostream>
#include <map>
#include <memory>
#include <vector>
 
#include "Geometry/Boundaries/Range.h"
#include "MPLibrary/ValidityCheckers/CollisionDetection/CDInfo.h"
#include "Utilities/MPUtils.h"
 
#include "Vector.h"
 
class MultiBody;
class Boundary;
class Environment;
class Robot;
 
enum class DofType;
 
namespace mathtool {
  class EulerAngle;
  class Transformation;
}
 
 
class Cfg {
 
  public:
 
 
    explicit Cfg();
 
    explicit Cfg(Robot* const _robot);
 
    explicit Cfg(const mathtool::Vector3d& _v, Robot* const _robot = nullptr);
    
    Cfg(const Cfg& _other);
 
    Cfg(Cfg&& _other);
 
    virtual ~Cfg();
 
 
    Cfg& operator=(const Cfg& _cfg);
 
    Cfg& operator=(Cfg&& _cfg);
 
    Cfg& operator+=(const Cfg& _cfg);
    Cfg& operator-=(const Cfg& _cfg);
    Cfg& operator*=(const Cfg& _cfg);
    Cfg& operator/=(const Cfg& _cfg);
    Cfg& operator*=(const double _d);
    Cfg& operator/=(const double _d);
 
 
    Cfg operator-() const;
 
    Cfg operator+(const Cfg& _cfg) const;
 
    Cfg operator-(const Cfg& _cfg) const;
 
    Cfg operator*(const Cfg& _cfg) const;
 
    Cfg operator/(const Cfg& _cfg) const;
 
    Cfg operator*(const double _d) const;
 
    Cfg operator/(const double _d) const;
 
 
    bool operator==(const Cfg& _cfg) const;
 
    bool operator!=(const Cfg& _cfg) const;
 
    bool WithinResolution(const Cfg& _cfg, const double _posRes,
                          const double _oriRes) const;
 
 
    bool operator<(const Cfg& _cfg) const;
 
 
    Robot* GetRobot() const noexcept;
 
    void SetRobot(Robot* const _r) noexcept;
 
    MultiBody* GetMultiBody() const noexcept;
 
    size_t DOF() const noexcept;
 
    size_t PosDOF() const noexcept;
 
    size_t OriDOF() const noexcept;
 
    size_t JointDOF() const noexcept;
 
    bool IsNonholonomic() const noexcept;
 
 
    double& operator[](const size_t _dof) noexcept;
    double operator[](const size_t _dof) const noexcept;
 
    const std::vector<double>& GetData() const noexcept;
 
    double& Velocity(const size_t _dof) noexcept;
    double Velocity(const size_t _dof) const noexcept;
 
    const std::vector<double>& GetVelocity() const noexcept;
 
    virtual void SetData(const std::vector<double>& _data);
    virtual void SetData(std::vector<double>&& _data);
 
    void SetJointData(const std::vector<double>& _data);
 
    Point3d GetPoint() const noexcept;
 
    virtual std::vector<double> GetPosition() const;
    virtual std::vector<double> GetRotation() const;
    virtual std::vector<double> GetJoints() const;
    virtual std::vector<double> GetNonJoints() const;
    virtual std::vector<double> GetOrientation() const;
 
    virtual double Magnitude() const;
    virtual double PositionMagnitude() const;
    virtual double OrientationMagnitude() const;
 
    mathtool::Vector3d GetLinearPosition() const;
    mathtool::Vector3d GetAngularPosition() const;
    mathtool::EulerAngle GetEulerAngle() const;
    mathtool::Vector3d GetLinearVelocity() const;
    mathtool::Vector3d GetAngularVelocity() const;
    mathtool::Transformation GetBaseTransformation() const;
 
    void SetLinearPosition(const mathtool::Vector3d&);
    void SetAngularPosition(const mathtool::Vector3d&);
    void SetEulerAngle(const mathtool::EulerAngle&);
    void SetLinearVelocity(const mathtool::Vector3d&);
    void SetAngularVelocity(const mathtool::Vector3d&);
    void SetBaseTransformation(const mathtool::Transformation&);
 
    void TransformCfg(const mathtool::Transformation&);
 
 
    bool GetLabel(const std::string& _label) const;
    bool IsLabel(const std::string& _label) const noexcept;
    void SetLabel(const std::string& _label, const bool _value) noexcept;
 
    double GetStat(const std::string& _stat) const;
    bool IsStat(const std::string& _stat) const noexcept;
    void SetStat(const std::string& _stat, const double _value = 0) noexcept;
    void IncrementStat(const std::string& _stat, const double _value = 1)
        noexcept;
 
 
    void Zero() noexcept;
 
    bool InBounds(const Boundary* const _b) const noexcept;
    bool InBounds(const Environment* const _env) const noexcept;
 
    virtual void GetRandomCfg(const Boundary* const _b);
    virtual void GetRandomCfg(Environment* _env);
 
    virtual void GetRandomVelocity();
 
    template <typename DistanceMetricPointer>
    void GetRandomRay(const double _length, DistanceMetricPointer _dm,
        const bool _norm = true);
 
    virtual void ConfigureRobot() const;
 
    virtual void IncrementTowardsGoal(const Cfg& _goal, const Cfg& _increment);
 
    virtual void FindIncrement(const Cfg& _start, const Cfg& _goal,
        const int _nTicks);
 
    virtual void FindIncrement(const Cfg& _start, const Cfg& _goal, int* _nTicks,
        const double _positionRes, const double _orientationRes);
 
    virtual void WeightedSum(const Cfg& _c1, const Cfg& _c2,
        const double _weight = .5);
 
    virtual void GetPositionOrientationFrom2Cfg(const Cfg& _pos, const Cfg& _ori);
 
 
    std::vector<double> DirectionInLocalFrame(const Cfg& _target) const;
 
 
    // Static pointer for reading roadmaps. It should be set to the relevant
    // robot before reading in the map file, and nullptr otherwise.
    static Robot* inputRobot;
 
    virtual void Read(std::istream& _is);
 
    virtual void Write(std::ostream& _os) const;
 
    std::string PrettyPrint(const size_t _precision = 4) const;
 
    std::string ToString() const;
    
 
    CDInfo m_clearanceInfo;
    std::shared_ptr<Cfg> m_witnessCfg;
 
 
    void EnableNormalization() const;
 
    void DisableNormalization() const;
 
    virtual void NormalizeOrientation(const int _index = -1) noexcept;
 
    void EnforceVelocityLimits() noexcept;
 
 
  protected:
 
 
    std::vector<double> m_dofs;    
    std::vector<double> m_vel;     
    Robot* m_robot{nullptr};       
 
    std::map<std::string, bool> m_labelMap;  
    std::map<std::string, double> m_statMap; 
 
    mutable double (*m_normalizer)(const double&){Normalize};
 
 
};
 
 
/*--------------------------- Generation Methods -----------------------------*/
 
template <class DistanceMetricPointer>
void
Cfg::
GetRandomRay(const double _length, DistanceMetricPointer _dm, const bool _norm) {
  // Randomly sample DOFs.
  for(size_t i = 0; i < DOF(); ++i)
    m_dofs[i] = GRand();
 
  // Scale to appropriate length.
  _dm->ScaleCfg(_length, *this);
 
  // Normalize if requested.
  if(_norm)
    NormalizeOrientation();
}
 
/*----------------------------------------------------------------------------*/
 
std::ostream& operator<<(std::ostream& _os, const Cfg& _cfg);
std::istream& operator>>(std::istream& _is, Cfg& _cfg);
 
#endif