sto_policy.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_STO_POLICY_HPP_
#define ROBOTOC_STO_POLICY_HPP_
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
 
 
namespace robotoc {
 
class STOPolicy {
public:
  STOPolicy(const Robot& robot)
    : dtsdx(Eigen::VectorXd::Zero(2*robot.dimv())),
      dtsdts(0),
      dts0(0) {
  }
 
  STOPolicy() 
    : dtsdx(),
      dtsdts(0),
      dts0(0) {
  }
 
  ~STOPolicy() {
  }
 
  STOPolicy(const STOPolicy&) = default;
 
  STOPolicy& operator=(const STOPolicy&) = default;
 
  STOPolicy(STOPolicy&&) noexcept = default;
 
  STOPolicy& operator=(STOPolicy&&) noexcept = default;
 
  Eigen::VectorXd dtsdx;
 
  double dtsdts;
 
  double dts0;
 
  bool isApprox(const STOPolicy& other) const {
    if (!dtsdx.isApprox(other.dtsdx)) return false;
    Eigen::Vector2d vec, vec_other;
    vec << dtsdts, dts0;
    vec_other << other.dtsdts, other.dts0;
    if (!vec.isApprox(vec_other)) return false;
    return true;
  }
 
private:
 
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_STO_POLICY_HPP_