Commit 0a4eb023 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Fix MAL.

parent 2cf74dfe
Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
Subproject commit 8474524906099169b05f5c2d0523519d8e383df1
......@@ -33,6 +33,7 @@ frame work */
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include <jrl/mal/matrixabstractlayer.hh>
namespace PatternGeneratorJRL
{
struct PinocchioRobotFoot_t{
......@@ -40,7 +41,7 @@ namespace PatternGeneratorJRL
double soleDepth ; // z axis
double soleWidth ; // y axis
double soleHeight ;// x axis
vector3d anklePosition ;
Eigen::Vector3d anklePosition ;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
typedef PinocchioRobotFoot_t PRFoot ;
......@@ -57,12 +58,12 @@ namespace PatternGeneratorJRL
/// Functions computing kinematics and dynamics
void computeInverseDynamics();
void computeInverseDynamics(MAL_VECTOR_TYPE(double) & q,
MAL_VECTOR_TYPE(double) & v,
MAL_VECTOR_TYPE(double) & a);
void computeInverseDynamics(Eigen::VectorXd & q,
Eigen::VectorXd & v,
Eigen::VectorXd & a);
void computeForwardKinematics();
void computeForwardKinematics(MAL_VECTOR_TYPE(double) & q);
void computeForwardKinematics(Eigen::VectorXd & q);
void RPYToSpatialFreeFlyer(Eigen::Vector3d & rpy,
Eigen::Vector3d & drpy,
......@@ -89,9 +90,9 @@ namespace PatternGeneratorJRL
virtual bool ComputeSpecializedInverseKinematics(
const se3::JointIndex &jointRoot,
const se3::JointIndex &jointEnd,
const MAL_S4x4_MATRIX_TYPE(double) & jointRootPosition,
const MAL_S4x4_MATRIX_TYPE(double) & jointEndPosition,
MAL_VECTOR_TYPE(double) &q);
const Eigen::Matrix4d & jointRootPosition,
const Eigen::Matrix4d & jointEndPosition,
Eigen::VectorXd &q);
///
/// \brief testInverseKinematics :
......@@ -118,14 +119,14 @@ namespace PatternGeneratorJRL
private :
// needed for the inverse geometry (ComputeSpecializedInverseKinematics)
void getWaistFootKinematics(const matrix4d & jointRootPosition,
const matrix4d & jointEndPosition,
vectorN &q,
vector3d Dt);
void getWaistFootKinematics(const Eigen::Matrix4d & jointRootPosition,
const Eigen::Matrix4d & jointEndPosition,
Eigen::VectorXd &q,
Eigen::Vector3d Dt);
double ComputeXmax(double & Z);
void getShoulderWristKinematics(const matrix4d & jointRootPosition,
const matrix4d & jointEndPosition,
vectorN &q,
void getShoulderWristKinematics(const Eigen::Matrix4d & jointRootPosition,
const Eigen::Matrix4d & jointEndPosition,
Eigen::VectorXd &q,
int side);
void DetectAutomaticallyShoulders();
void DetectAutomaticallyOneShoulder(se3::JointIndex aWrist,
......@@ -163,17 +164,17 @@ namespace PatternGeneratorJRL
inline se3::JointModelVector & getActuatedJoints()
{return m_robotModel->joints;}
inline MAL_VECTOR_TYPE(double) currentConfiguration()
inline Eigen::VectorXd currentConfiguration()
{return m_qmal;}
inline MAL_VECTOR_TYPE(double) currentVelocity()
inline Eigen::VectorXd currentVelocity()
{return m_vmal;}
inline MAL_VECTOR_TYPE(double) currentAcceleration()
inline Eigen::VectorXd currentAcceleration()
{return m_amal;}
inline unsigned numberDof()
{return m_robotModel->nv;}
inline void zeroMomentumPoint(MAL_S3_VECTOR_TYPE(double) & zmp)
inline void zeroMomentumPoint(Eigen::Vector3d & zmp)
{
m_externalForces = m_robotData->liMi[1].act(m_robotData->f[1]);
m_f = m_externalForces.linear() ;
......@@ -183,16 +184,16 @@ namespace PatternGeneratorJRL
zmp(2) = 0.0 ; // by default
}
inline void positionCenterOfMass(MAL_S3_VECTOR_TYPE(double) & com)
inline void positionCenterOfMass(Eigen::Vector3d & com)
{
m_com = m_robotData->com[0] ;
com(0) = m_com(0) ;
com(1) = m_com(1) ;
com(2) = m_com(2) ;
}
inline void CenterOfMass(MAL_S3_VECTOR_TYPE(double) & com,
MAL_S3_VECTOR_TYPE(double) & dcom,
MAL_S3_VECTOR_TYPE(double) & ddcom)
inline void CenterOfMass(Eigen::Vector3d & com,
Eigen::Vector3d & dcom,
Eigen::Vector3d & ddcom)
{
m_com = m_robotData->acom[0] ;
ddcom(0) = m_com(0) ;
......@@ -212,11 +213,11 @@ namespace PatternGeneratorJRL
/// SETTERS
/// ///////
inline void currentConfiguration(MAL_VECTOR_TYPE(double) conf)
inline void currentConfiguration(Eigen::VectorXd conf)
{m_qmal=conf;}
inline void currentVelocity(MAL_VECTOR_TYPE(double) vel)
inline void currentVelocity(Eigen::VectorXd vel)
{m_vmal=vel;}
inline void currentAcceleration(MAL_VECTOR_TYPE(double) acc)
inline void currentAcceleration(Eigen::VectorXd acc)
{m_amal=acc;}
/// Initialization functions
......@@ -242,9 +243,9 @@ namespace PatternGeneratorJRL
se3::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
se3::JointIndex m_leftWrist , m_rightWrist ;
MAL_VECTOR_TYPE(double) m_qmal ;
MAL_VECTOR_TYPE(double) m_vmal ;
MAL_VECTOR_TYPE(double) m_amal ;
Eigen::VectorXd m_qmal ;
Eigen::VectorXd m_vmal ;
Eigen::VectorXd m_amal ;
Eigen::VectorXd m_q ;
Eigen::VectorXd m_v ;
Eigen::VectorXd m_a ;
......@@ -265,7 +266,7 @@ namespace PatternGeneratorJRL
bool m_isArmInverseKinematic ;
// length between the waist and the hip
MAL_S3_VECTOR_TYPE(double) m_leftDt, m_rightDt ;
Eigen::Vector3d m_leftDt, m_rightDt ;
double m_femurLength ;
double m_tibiaLengthZ ;
double m_tibiaLengthY ;
......
......@@ -207,7 +207,6 @@ ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES} ${${PROJECT_NAME}_ABSOLUTE_HEADERS
# Define dependencies
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-msse -msse2 -msse3 -march=core2 -mfpmath=sse -fivopts -ftree-loop-im -fipa-pta ")
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} jrl-mal)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} pinocchio)
IF(USE_LSSOL)
......
......@@ -559,7 +559,7 @@ namespace PatternGeneratorJRL {
memset(&InitLeftFootAbsPos,0,sizeof(InitLeftFootAbsPos));
memset(&InitRightFootAbsPos,0,sizeof(InitRightFootAbsPos));
MAL_VECTOR(lStartingWaistPose,double);
Eigen::VectorXd lStartingWaistPose;
EvaluateStartingState(lStartingCOMState,
lStartingZMPPosition,
......@@ -602,7 +602,7 @@ namespace PatternGeneratorJRL {
memset(&InitLeftFootAbsPos,0,sizeof(InitLeftFootAbsPos));
memset(&InitRightFootAbsPos,0,sizeof(InitRightFootAbsPos));
MAL_VECTOR(lStartingWaistPose,double);
Eigen::VectorXd lStartingWaistPose;
EvaluateStartingState(lStartingCOMState,
lStartingZMPPosition,
......@@ -659,7 +659,7 @@ namespace PatternGeneratorJRL {
void PatternGeneratorInterfacePrivate::EvaluateStartingCOM(MAL_VECTOR( & Configuration,double),
MAL_S3_VECTOR( & lStartingCOMState,double))
{
MAL_VECTOR(Velocity,double);
Eigen::VectorXd Velocity;
MAL_VECTOR_RESIZE(Velocity,MAL_VECTOR_SIZE(Configuration));
for(unsigned int i=0;i<MAL_VECTOR_SIZE(Configuration);i++)
Velocity[i] = 0.0;
......@@ -673,11 +673,11 @@ namespace PatternGeneratorJRL {
void PatternGeneratorInterfacePrivate::EvaluateStartingState(COMState & lStartingCOMState,
MAL_S3_VECTOR_TYPE(double) & lStartingZMPPosition,
MAL_VECTOR_TYPE(double) & lStartingWaistPose,
Eigen::VectorXd & lStartingWaistPose,
FootAbsolutePosition & InitLeftFootAbsPos,
FootAbsolutePosition & InitRightFootAbsPos)
{
MAL_VECTOR(lBodyInit,double);
Eigen::VectorXd lBodyInit;
MAL_VECTOR_RESIZE(lBodyInit,m_CurrentActuatedJointValues.size());
for(unsigned int j=0; j<m_CurrentActuatedJointValues.size();j++)
......@@ -810,7 +810,7 @@ namespace PatternGeneratorJRL {
// Initialize consequently the ComAndFoot Realization object.
MAL_VECTOR(lStartingWaistPose,double);
Eigen::VectorXd lStartingWaistPose;
m_GlobalStrategyManager->EvaluateStartingState(BodyAnglesIni,
lStartingCOMState,
lStartingZMPPosition,
......@@ -983,7 +983,7 @@ namespace PatternGeneratorJRL {
MAL_VECTOR_TYPE(double) lCurrentConfiguration;
Eigen::VectorXd lCurrentConfiguration;
lCurrentConfiguration = m_PinocchioRobot->currentConfiguration();
......@@ -1306,9 +1306,9 @@ namespace PatternGeneratorJRL {
bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration,
MAL_VECTOR( &ZMPTarget,double))
{
COMState finalCOMState;
......@@ -1329,9 +1329,9 @@ namespace PatternGeneratorJRL {
ZMPPosition &ZMPRefPos,
COMPosition &COMRefPos)
{
MAL_VECTOR_TYPE(double) CurrentConfiguration;
MAL_VECTOR_TYPE(double) CurrentVelocity;
MAL_VECTOR_TYPE(double) CurrentAcceleration;
Eigen::VectorXd CurrentConfiguration;
Eigen::VectorXd CurrentVelocity;
Eigen::VectorXd CurrentAcceleration;
MAL_VECTOR( ZMPTarget,double);
COMState aCOMRefState;
......@@ -1351,9 +1351,9 @@ namespace PatternGeneratorJRL {
}
bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration,
MAL_VECTOR( &ZMPTarget,double),
COMPosition &finalCOMPosition,
FootAbsolutePosition &LeftFootPosition,
......@@ -1371,9 +1371,9 @@ namespace PatternGeneratorJRL {
return m_Running;
}
bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
bool PatternGeneratorInterfacePrivate::RunOneStepOfTheControlLoop(Eigen::VectorXd & CurrentConfiguration,
Eigen::VectorXd & CurrentVelocity,
Eigen::VectorXd & CurrentAcceleration,
MAL_VECTOR( &ZMPTarget,double),
COMState &finalCOMState,
FootAbsolutePosition &LeftFootPosition,
......
......@@ -43,14 +43,14 @@ LinearizedInvertedPendulum2D::LinearizedInvertedPendulum2D()
m_ComHeight = -1.0;
m_SamplingPeriod = -1.0;
m_InterpolationInterval = -1;
MAL_MATRIX_RESIZE(m_A,3,3);
MAL_MATRIX_RESIZE(m_B,3,1);
MAL_MATRIX_RESIZE(m_C,1,3);
m_A.resize(3,3);
m_B.resize(3,1);
m_C.resize(1,3);
MAL_VECTOR_RESIZE(m_xk,6);
MAL_VECTOR_RESIZE(m_CoM.x,3);
MAL_VECTOR_RESIZE(m_CoM.y,3);
MAL_VECTOR_RESIZE(m_zk,2);
m_xk.resize(6);
m_CoM.x.resize(3);
m_CoM.y.resize(3);
m_zk.resize(2);
RESETDEBUG4("Debug2DLIPM.dat");
}
......@@ -105,7 +105,7 @@ void LinearizedInvertedPendulum2D::SetRobotControlPeriod(const double & aT)
}
void LinearizedInvertedPendulum2D::GetState(MAL_VECTOR_TYPE(double) &lxk)
void LinearizedInvertedPendulum2D::GetState(Eigen::VectorXd &lxk)
{
//For compability reasons
m_xk[0] = m_CoM.x[0];m_xk[1] = m_CoM.x[1];m_xk[2] = m_CoM.x[2];
......@@ -258,8 +258,8 @@ int LinearizedInvertedPendulum2D::Interpolation(deque<COMState> &COMStates,
com_t LinearizedInvertedPendulum2D::OneIteration(double ux, double uy)
{
MAL_VECTOR_DIM(Bux,double,3);
MAL_VECTOR_DIM(Buy,double,3);
Eigen::VectorXd Bux(3);
Eigen::VectorXd Buy(3);
Bux[0] = ux*m_B(0,0);
Bux[1] = ux*m_B(1,0);
......@@ -270,15 +270,12 @@ com_t LinearizedInvertedPendulum2D::OneIteration(double ux, double uy)
Buy[2] = uy*m_B(2,0);
// Simulate the dynamical system
m_CoM.x = MAL_RET_A_by_B(m_A,m_CoM.x);
m_CoM.x = m_A*m_CoM.x;
m_CoM.x = m_CoM.x + Bux;
m_CoM.y = MAL_RET_A_by_B(m_A,m_CoM.y);
m_CoM.y = m_A*m_CoM.y;
m_CoM.y = m_CoM.y + Buy;
// Modif. from Dimitar: Initially a mistake regarding the ordering.
//MAL_C_eq_A_by_B(m_zk,m_C,m_xk);
ODEBUG4( m_xk[0] << " " << m_xk[1] << " " << m_xk[2] << " " <<
m_xk[3] << " " << m_xk[4] << " " << m_xk[5] << " " <<
m_CoM.x << " " << m_CoM.y << " " <<
......
......@@ -101,20 +101,20 @@ namespace PatternGeneratorJRL
@{
*/
/* ! Matrix regarding the state of the CoM (pos, velocity, acceleration) */
MAL_MATRIX(m_A,double);
Eigen::MatrixXd m_A;
/* ! Vector for the command */
MAL_MATRIX(m_B,double);
Eigen::MatrixXd m_B;
/* ! Vector for the ZMP. */
MAL_MATRIX(m_C,double);
Eigen::MatrixXd m_C;
/*! \brief State of the LIPM at the \f$k\f$ eme iteration
\f$ x_k = [ c_x \dot{c}_x \ddot{c}_x c_y \dot{c}_y \ddot{c}_y\f$ */
MAL_VECTOR(m_xk,double);
Eigen::VectorXd m_xk;
com_t m_CoM;
/* ! \brief Vector of ZMP */
MAL_VECTOR(m_zk,double);
Eigen::VectorXd m_zk;
/* ! @} */
......@@ -150,7 +150,7 @@ namespace PatternGeneratorJRL
{m_CoM = CoM;};
/*! Get state. */
void GetState(MAL_VECTOR_TYPE(double) &lxk);
void GetState(Eigen::VectorXd &lxk);
COMState GetState();
inline com_t getState()
......
......@@ -95,9 +95,9 @@ logical sb02ow (double *_alphar, double * /* _alphai */, double *_beta)
}
OptimalControllerSolver::OptimalControllerSolver(MAL_MATRIX( &A, double),
MAL_MATRIX( &b, double),
MAL_MATRIX( &c, double),
OptimalControllerSolver::OptimalControllerSolver(Eigen::MatrixXd &A, double),
Eigen::MatrixXd &b, double),
Eigen::MatrixXd &c, double),
double Q, double R,
unsigned int Nl)
{
......@@ -130,13 +130,13 @@ OptimalControllerSolver::~OptimalControllerSolver()
}
bool OptimalControllerSolver::GeneralizedSchur(MAL_MATRIX( &A,double),
MAL_MATRIX( &B,double),
bool OptimalControllerSolver::GeneralizedSchur(Eigen::MatrixXd &A,double),
Eigen::MatrixXd &B,double),
MAL_VECTOR( &alphar,double),
MAL_VECTOR( &alphai,double),
MAL_VECTOR( &beta,double),
MAL_MATRIX( &L,double),
MAL_MATRIX( &R,double))
Eigen::MatrixXd &L,double),
Eigen::MatrixXd &R,double))
{
ODEBUG("A:" << A);
ODEBUG("B:" << A);
......@@ -207,8 +207,8 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
// [-c'Qc E]]
// ODEBUG("m_A:" << m_A);
// And each sub-matrix is n x n matrix.
MAL_MATRIX(H,double);
MAL_MATRIX(tm_b,double);
Eigen::MatrixXd H;
Eigen::MatrixXd tm_b;
// Store the transpose of m_b;
tm_b = MAL_RET_TRANSPOSE(m_b);
......@@ -227,7 +227,7 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
for(int j=0;j<n;j++)
H(i,j) = m_A(i,j);
MAL_MATRIX(H21,double);
Eigen::MatrixXd H21;
H21 = MAL_RET_TRANSPOSE(m_c);
ODEBUG("H21 (1):" << H21);
H21 = H21 * m_Q;
......@@ -245,10 +245,10 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
MAL_MATRIX_DIM(E,double,2*n,2*n);
MAL_MATRIX_SET_IDENTITY(E);
MAL_MATRIX(G,double);
Eigen::MatrixXd G;
G = MAL_RET_A_by_B(m_b * (1/m_R) , tm_b);
MAL_MATRIX(At,double);
Eigen::MatrixXd At;
At= MAL_RET_TRANSPOSE(m_A);
for(int i=0;i< n;i++)
for(int j=0;j<n;j++)
......@@ -277,7 +277,7 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
// Computes P the solution of the Riccati equation.
MAL_MATRIX(P,double);
Eigen::MatrixXd P;
MAL_MATRIX_DIM(Z11,double,n,n);
MAL_MATRIX_DIM(Z21,double,n,n);
......@@ -293,14 +293,14 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
ODEBUG( "Z11:" << endl << Z11 << endl
<< "Z21:" << endl << Z21 );
MAL_MATRIX(iZ11,double);
MAL_INVERSE(Z11,iZ11,double);
Eigen::MatrixXd iZ11;
MAL_INVERSE(Z11,iZ11;
P = MAL_RET_A_by_B(Z21, iZ11);
ODEBUG( "P: " << endl << P);
// Compute the weights.
MAL_MATRIX(r,double);
Eigen::MatrixXd r;
double la;
r = tm_b; // b^T
......@@ -318,11 +318,11 @@ void OptimalControllerSolver::ComputeWeights(unsigned int Mode)
ODEBUG("K: "<< endl << m_K);
// Computes the weights for the future.
MAL_MATRIX(PreMatrix,double);
MAL_MATRIX(Recursive,double);
MAL_MATRIX(BaseOfRecursion,double);
MAL_MATRIX(PostMatrix,double);
MAL_MATRIX(Intermediate,double);
Eigen::MatrixXd PreMatrix;
Eigen::MatrixXd Recursive;
Eigen::MatrixXd BaseOfRecursion;
Eigen::MatrixXd PostMatrix;
Eigen::MatrixXd Intermediate;
PreMatrix = la * tm_b;
BaseOfRecursion = m_A - MAL_RET_A_by_B(m_b , m_K);
......@@ -357,12 +357,12 @@ void OptimalControllerSolver::DisplayWeights()
std::cout << "F:" << m_F << std::endl;
}
void OptimalControllerSolver::GetF(MAL_MATRIX(& lF, double))
void OptimalControllerSolver::GetF(Eigen::MatrixXd & lF, double))
{
lF = m_F;
}
void OptimalControllerSolver::GetK(MAL_MATRIX(& lK,double))
void OptimalControllerSolver::GetK(Eigen::MatrixXd & lK,double))
{
lK = m_K;
}
......@@ -144,9 +144,9 @@ The augmented system is then
static const unsigned int MODE_WITH_INITIALPOS=0;
/*! A constructor */
OptimalControllerSolver(MAL_MATRIX(&A,double),
MAL_MATRIX(&b,double),
MAL_MATRIX(&c,double),
OptimalControllerSolver(Eigen::MatrixXd &A,
Eigen::MatrixXd &b,
Eigen::MatrixXd &c,
double Q, double R,
unsigned int Nl);
......@@ -162,19 +162,19 @@ The augmented system is then
/*! Display the weights */
void DisplayWeights();
bool GeneralizedSchur(MAL_MATRIX(&A,double),
MAL_MATRIX(&B,double),
MAL_VECTOR(&alphar,double),
MAL_VECTOR(&alphai,double),
MAL_VECTOR(&beta,double),
MAL_MATRIX(&L,double),
MAL_MATRIX(&R,double));
bool GeneralizedSchur(Eigen::MatrixXd &A,
Eigen::MatrixXd &B,
Eigen::VectorXd &alphar,
Eigen::VectorXd &alphai,
Eigen::VectorXd &beta,
Eigen::MatrixXd &L,
Eigen::MatrixXd &R);
/*! To take matrix F aka the weights of the preview window . */
void GetF(MAL_MATRIX(& LF,double) );
void GetF(Eigen::MatrixXd & LF );
/*! To take matrix K, aka the weight of the other part of the command */
void GetK(MAL_MATRIX(& LK,double) );
void GetK(Eigen::MatrixXd & LK );
protected:
......@@ -186,9 +186,9 @@ The augmented system is then
\f}
*/
MAL_MATRIX(m_A,double);
MAL_MATRIX(m_b,double);
MAL_MATRIX(m_c,double);
Eigen::MatrixXd m_A;
Eigen::MatrixXd m_b;
Eigen::MatrixXd m_c;
/*! The coefficent of the index criteria:
\f[
......@@ -199,8 +199,8 @@ The augmented system is then
/*! The weights themselves */
MAL_MATRIX(m_K,double);
MAL_MATRIX(m_F,double);
Eigen::MatrixXd m_K;
Eigen::MatrixXd m_F;
/*! The size of the window for the preview */
int m_Nl;
......
......@@ -51,11 +51,11 @@ PreviewControl::PreviewControl(SimplePluginManager *lSPM,
m_Zc = 0.0;
m_SizeOfPreviewWindow = 0;
MAL_MATRIX_RESIZE(m_A,3,3);
MAL_MATRIX_RESIZE(m_B,3,1);
MAL_MATRIX_RESIZE(m_C,1,3);
Eigen::MatrixXd m_A.resize(3,3);
Eigen::MatrixXd m_B.resize(3,1);
Eigen::MatrixXd m_C.resize(1,3);
MAL_MATRIX_RESIZE(m_Kx,1,3);
Eigen::MatrixXd m_Kx.resize(1,3);
m_Ks = 0;
......@@ -166,7 +166,7 @@ void PreviewControl::ReadPrecomputedFile(string aFileName)
m_SizeOfPreviewWindow = (unsigned int)(m_PreviewControlTime/
m_SamplingPeriod);
MAL_MATRIX_RESIZE(m_F,m_SizeOfPreviewWindow,1);
Eigen::MatrixXd m_F.resize(m_SizeOfPreviewWindow,1);
for(unsigned int i=0;i<m_SizeOfPreviewWindow;i++)
{
......@@ -216,7 +216,7 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
m_C(0,2) = -m_Zc/9.81;
ODEBUG(" m_Zc: " << m_Zc << " m_C(0,2)" << m_C(0,2));
MAL_MATRIX_TYPE( double) lF,lK;
Eigen::MatrixXd lF,lK;
double Q=0.0,R=0.0;
int Nl;
......@@ -236,14 +236,14 @@ void PreviewControl::ComputeOptimalWeights(unsigned int mode)
R = 1e-6;
// Build the derivated system