From 00e1f3b356ede36ec06ccee424b9f909c6c9a042 Mon Sep 17 00:00:00 2001
From: asctec <asctec@kinton.users.iri.prv>
Date: Wed, 5 Aug 2015 12:16:22 +0200
Subject: [PATCH] added eigen stuff with std vector@robot
---
src/quadarm_task_priority_ctrl.cpp | 12 ++++++++----
1 file changed, 8 insertions(+), 4 deletions(-)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index c0a0c7d..864baa7 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);
--
GitLab