From 14b1437d9d257791448ab1e6f433ec3b8fdd7970 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Wed, 5 Aug 2015 18:15:30 +0200
Subject: [PATCH] VS working with IBVS. no secondary tasks. No Weighted
Pseudoinverse. No saturations
---
src/quadarm_task_priority_ctrl.cpp | 11 +++++++----
1 file changed, 7 insertions(+), 4 deletions(-)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 8c1c992..d6a8207 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -291,7 +291,10 @@ void CQuadarm_Task_Priority_Ctrl::uam_control(){
// Task 0 + 1 + 3
//Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel;
// All tasks
- Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel;
+ //Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel;
+
+//TODO
+Vtotal = JVS_wpseudo * sigmaVS;
Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
@@ -360,10 +363,10 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
JVS = Jqa1;
// task jacobian pseudo inverse
- //JVS_pseudo = calcPinv(Jqa1);
- JVS_pseudo = weight_jacvs_inverse(Jqa1);
+ JVS_pseudo = calcPinv(Jqa1);
+ //JVS_pseudo = weight_jacvs_inverse(Jqa1);
- //std::cout << "No weighted pseudo inverse: @line:" << __LINE__ << std::endl;
+//TODO
// task velocity vector
sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;
--
GitLab