From d997fafde263a9d42cc3e70d57da52d252bf9bdd Mon Sep 17 00:00:00 2001
From: asantamaria <somriu@gmail.com>
Date: Tue, 19 May 2015 10:18:01 +0200
Subject: [PATCH] rearranging vs task to a single function
---
src/quadarm_task_priority_ctrl.cpp | 7 +++++--
1 file changed, 5 insertions(+), 2 deletions(-)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 6a13c7a..6db4225 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -201,7 +201,7 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
void CQuadarm_Task_Priority_Ctrl::qa_control(){
// Underactuated Quadrotor
- MatrixXd J1(8,4),J2(8,2),V1(4,1),Jqa1(6,4+this->arm_.nj),Jqa2(6,2);
+ MatrixXd J1(8,4),J2(8,2),Jqa1(6,4+this->arm_.nj),Jqa2(6,2);
// 9 DOF Extracting wx and wy from quadrotor //////////////////
Jqa1.block(0,0,6,3) = this->Jqa_.block(0,0,6,3);
@@ -217,6 +217,9 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
MatrixXd Jqa1_wpseudo(4+this->arm_.nj,6);
Jqa1_wpseudo = weight_jacvs_inverse(Jqa1);
+ MatrixXd sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
+
+
// Control law ________________________________________________________________
// Secutiry task (Inflation radius)
@@ -225,7 +228,7 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
// Primary control task (Visual Servo) ________________________
- MatrixXd sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
+
this->ctrl_params_.vs_vel = Jqa1_wpseudo * sigmaVS;
--
GitLab