add costFunction specialized for romeo actuator

qtTmp/costfunctionromeoactuator.cpp

+#include "costfunctionromeoactuator.h"
+
+CostFunctionRomeoActuator::CostFunctionRomeoActuator()
+{
+    this->Q << 100.0,0.0,0.0,0.0,
+                0.0,0.0,0.0,0.0,
+                0.0,0.0,0.0,0.0,
+                0.0,0.0,0.0,0.0;
+    this->R = 0.1;
+
+    this->lxx = this->Q;
+    this->luu = this->R;
+    this->lux << 0.0,0.0,0.0,0.0;
+    this->lxu << 0.0,0.0,0.0,0.0;
+}
+
+void CostFunctionRomeoActuator::computeAllCostDeriv(V4 &X, V4 &Xdes, double &U)
+{
+    this->lx = this->Q*(X-Xdes);
+    this->lu = this->R*U;
+}
+
+void CostFunctionRomeoActuator::commuteFinalCostDeriv(V4 &X, V4 &Xdes)
+{
+    this->lx = this->Q*(X-Xdes);
+}
+
+// accessors //
+V4 CostFunctionRomeoActuator::getlx()
+{
+    return this->lx;
+}
+
+M4 CostFunctionRomeoActuator::getlxx()
+{
+    return this->lxx;
+}
+
+double CostFunctionRomeoActuator::getlu()
+{
+    return this->lu;
+}
+
+double CostFunctionRomeoActuator::getluu()
+{
+    return this->luu;
+}
+
+V4 CostFunctionRomeoActuator::getlxu()
+{
+    return this->lxu;
+}
+
+V4T CostFunctionRomeoActuator::getlux()
+{
+    return this->lux;
+}

qtTmp/costfunctionromeoactuator.h

+#ifndef COSTFUNCTIONROMEOACTUATOR_H
+#define COSTFUNCTIONROMEOACTUATOR_H
+
+#include <Eigen/Core>
+
+using namespace Eigen;
+
+typedef Matrix<double,4,4> M4;
+typedef Matrix<double,4,1> V4;
+typedef Matrix<double,1,4> V4T;
+
+class CostFunctionRomeoActuator
+{
+public:
+    CostFunctionRomeoActuator();
+private:
+    M4 Q;
+    double R;
+    V4 lx;
+    M4 lxx;
+    double lu,luu;
+    V4T lux;
+    V4 lxu;
+protected:
+    // attributes //
+public:
+private:
+
+protected:
+    // methods //
+public:
+    void computeAllCostDeriv(V4& X, V4& Xdes, double& U);
+    void commuteFinalCostDeriv(V4& X, V4& Xdes);
+private:
+protected:
+    // accessors //
+public:
+    V4 getlx();
+    M4 getlxx();
+    double getlu();
+    double getluu();
+    V4T getlux();
+    V4 getlxu();
+};
+
+#endif // COSTFUNCTIONROMEOACTUATOR_H