diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index c0a0c7de999374df9084c4506cb4ccc1f60f76a9..864baa73ab7ff6d845ed8cc377d190d9939a6c0e 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -1,5 +1,7 @@
 #include "quadarm_task_priority_ctrl.h"
 
+#include<Eigen/StdVector>
+
 using namespace Eigen;
 using namespace KDL;
 using namespace std;
@@ -23,7 +25,7 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
 																														 MatrixXd& robot_pos,
                                                              MatrixXd& robot_vel)
 {
-	
+
   // Current quadrotor height
   this->quad_height_ = quad_height;
 
@@ -201,7 +203,7 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
 
 // Control
 void CQuadarm_Task_Priority_Ctrl::uam_control(){
-  
+ 
   // TASK 0: Security task (Inflation radius) _____________________
 
   // task Jacobian and sigma
@@ -371,9 +373,11 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
   // KDL stuff
   vector<Frame> joint_pose(this->arm_.nj); // Vector of Joint frames
   joint_pose.resize(this->arm_.nj);
-  vector<Matrix4d> Transf(this->arm_.nj); // Vector of HT of frames relative to each links
-  Transf.resize(this->arm_.nj);
 
+  // This should be done, otherwise an eigen assertion arises in the robot computer
+  vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > Transf(this->arm_.nj);
+  //vector<Matrix4d> Transf(this->arm_.nj); // Vector of HT of frames relative to each links
+  Transf.resize(this->arm_.nj);
   // Eigen stuff
   vector<MatrixXd> T_base_to_joint(this->arm_.nj+1); // Vector of HT of frame links relative to base
   T_base_to_joint.resize(this->arm_.nj+1);