Commit ac181610 authored by flforget's avatar flforget
Browse files

add costFunction specialized for romeo actuator

parent c49e295c
#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;
}
#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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment