costfunction.hh
Go to the documentation of this file.
1 #ifndef COSTFUNCTION_H
2 #define COSTFUNCTION_H
3 
4 #include <Eigen/Core>
5 
6 template <typename precision, int stateSize, int commandSize>
7 class CostFunction {
8  public:
9  typedef Eigen::Matrix<precision, stateSize, 1> stateVec_t; // 1 x stateSize
10  typedef Eigen::Matrix<precision, 1, stateSize> stateVecTrans_t; // 1 x stateSize
11  typedef Eigen::Matrix<precision, stateSize, stateSize> stateMat_t; // stateSize x stateSize
12  typedef Eigen::Matrix<precision, stateSize, stateSize> stateTens_t[stateSize]; // stateSize x stateSize x stateSize
13 
14  // typedef for commandSize types
15  typedef Eigen::Matrix<precision, commandSize, 1> commandVec_t; // commandSize x 1
16  typedef Eigen::Matrix<precision, 1, commandSize> commandVecTrans_t; // 1 x commandSize
17  typedef Eigen::Matrix<precision, commandSize, commandSize> commandMat_t; // commandSize x commandSize
18  typedef Eigen::Matrix<precision, commandSize, commandSize>
19  commandTens_t[commandSize]; // stateSize x commandSize x commandSize
20 
21  // typedef for mixed stateSize and commandSize types
22  typedef Eigen::Matrix<precision, stateSize, commandSize> stateR_commandC_t; // stateSize x commandSize
23  typedef Eigen::Matrix<precision, stateSize, commandSize>
24  stateR_commandC_stateD_t[stateSize]; // stateSize x commandSize x stateSize
25  typedef Eigen::Matrix<precision, stateSize, commandSize>
26  stateR_commandC_commandD_t[commandSize]; // stateSize x commandSize x commandSize
27  typedef Eigen::Matrix<precision, commandSize, stateSize> commandR_stateC_t; // commandSize x stateSize
28  typedef Eigen::Matrix<precision, commandSize, stateSize>
29  commandR_stateC_stateD_t[stateSize]; // commandSize x stateSize x stateSize
30  typedef Eigen::Matrix<precision, commandSize, stateSize>
31  commandR_stateC_commandD_t[commandSize]; // commandSize x stateSize x commandSize
32  typedef Eigen::Matrix<precision, stateSize, stateSize>
33  stateR_stateC_commandD_t[commandSize]; // stateSize x stateSize x commandSize
34  typedef Eigen::Matrix<precision, commandSize, commandSize>
35  commandR_commandC_stateD_t[stateSize]; // commandSize x commandSize x stateSize
36 
37  public:
38  private:
39  protected:
40  // attributes //
41  public:
42  private:
43  protected:
44  double dt;
45  double final_cost;
46  double running_cost;
53  // methods //
54  public:
55  virtual void computeCostAndDeriv(const stateVec_t& X, const stateVec_t& Xdes, const commandVec_t& U) = 0;
56  virtual void computeFinalCostAndDeriv(const stateVec_t& X, const stateVec_t& Xdes) = 0;
57 
58  private:
59  protected:
60  // accessors //
61  public:
62  double& getRunningCost() { return running_cost; }
63  double& getFinalCost() { return final_cost; }
64  stateVec_t& getlx() { return lx; }
65  stateMat_t& getlxx() { return lxx; }
66  commandVec_t& getlu() { return lu; }
67  commandMat_t& getluu() { return luu; }
68  commandR_stateC_t& getlux() { return lux; }
69  stateR_commandC_t& getlxu() { return lxu; }
70 };
71 
72 #endif // COSTFUNCTION_H
Definition: costfunction.hh:7
commandR_stateC_t & getlux()
Definition: costfunction.hh:68
stateR_commandC_t lxu
Definition: costfunction.hh:52
virtual void computeFinalCostAndDeriv(const stateVec_t &X, const stateVec_t &Xdes)=0
double running_cost
Definition: costfunction.hh:46
stateMat_t & getlxx()
Definition: costfunction.hh:65
stateVec_t & getlx()
Definition: costfunction.hh:64
Eigen::Matrix< precision, 1, commandSize > commandVecTrans_t
Definition: costfunction.hh:16
Eigen::Matrix< precision, commandSize, commandSize > commandR_commandC_stateD_t[stateSize]
Definition: costfunction.hh:35
commandR_stateC_t lux
Definition: costfunction.hh:51
commandMat_t & getluu()
Definition: costfunction.hh:67
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_t
Definition: costfunction.hh:22
double & getFinalCost()
Definition: costfunction.hh:63
Eigen::Matrix< precision, commandSize, 1 > commandVec_t
Definition: costfunction.hh:15
double dt
Definition: costfunction.hh:44
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_t
Definition: costfunction.hh:27
Eigen::Matrix< precision, stateSize, 1 > stateVec_t
Definition: costfunction.hh:9
stateMat_t lxx
Definition: costfunction.hh:48
Eigen::Matrix< precision, commandSize, commandSize > commandMat_t
Definition: costfunction.hh:17
virtual void computeCostAndDeriv(const stateVec_t &X, const stateVec_t &Xdes, const commandVec_t &U)=0
commandVec_t lu
Definition: costfunction.hh:49
commandVec_t & getlu()
Definition: costfunction.hh:66
Eigen::Matrix< precision, 1, stateSize > stateVecTrans_t
Definition: costfunction.hh:10
Eigen::Matrix< precision, stateSize, stateSize > stateMat_t
Definition: costfunction.hh:11
commandMat_t luu
Definition: costfunction.hh:50
stateVec_t lx
Definition: costfunction.hh:47
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_commandD_t[commandSize]
Definition: costfunction.hh:31
stateR_commandC_t & getlxu()
Definition: costfunction.hh:69
Eigen::Matrix< precision, stateSize, stateSize > stateR_stateC_commandD_t[commandSize]
Definition: costfunction.hh:33
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_commandD_t[commandSize]
Definition: costfunction.hh:26
Eigen::Matrix< precision, commandSize, commandSize > commandTens_t[commandSize]
Definition: costfunction.hh:19
double & getRunningCost()
Definition: costfunction.hh:62
Eigen::Matrix< precision, commandSize, stateSize > commandR_stateC_stateD_t[stateSize]
Definition: costfunction.hh:29
Eigen::Matrix< precision, stateSize, stateSize > stateTens_t[stateSize]
Definition: costfunction.hh:12
double final_cost
Definition: costfunction.hh:45
Eigen::Matrix< precision, stateSize, commandSize > stateR_commandC_stateD_t[stateSize]
Definition: costfunction.hh:24