From 22eee28aedaf07c9843b0350c2f37bec1bbb4764 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Tue, 4 Aug 2015 20:37:48 +0200
Subject: [PATCH] still not working. shit!
---
src/quadarm_task_priority_ctrl.cpp | 10 ++++------
1 file changed, 4 insertions(+), 6 deletions(-)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 60c76a9..c0a0c7d 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -104,7 +104,6 @@ void CQuadarm_Task_Priority_Ctrl::uam_kinematics(){
// Whole robot Jacobian
this->uam_.jacob = MatrixXd::Zero(6,6+this->arm_.nj);
-
this->uam_.jacob.block(0,0,6,6) = Rot*Jb2c*Ji2b;
this->uam_.jacob.block(0,6,6,this->arm_.nj)=Rot*Ja;
}
@@ -120,9 +119,8 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics()
// resize the joint state vector in non-realtime
this->arm_.jnt_pos_kdl.resize(this->arm_.nj);
- for(unsigned int ii=0; ii < this->arm_.nj; ii++){
+ for(unsigned int ii=0; ii < this->arm_.nj; ii++)
this->arm_.jnt_pos_kdl.data(ii) = this->arm_.jnt_pos(ii,0);
- }
// computes Cartesian pose in realtime From Base_link to Arm tip
Frame kdlTarm;
@@ -164,7 +162,6 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::arm_jacobian()
Ja(ii,jj) = this->arm_.jacobian.data(ii,jj);
}
}
-
return Ja;
}
MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
@@ -356,15 +353,16 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
Jqa1.block(0,0,6,3) = this->uam_.jacob.block(0,0,6,3);
Jqa1.block(0,3,6,1+this->arm_.nj) = this->uam_.jacob.block(0,5,6,1+this->arm_.nj);
Jqa2 = this->uam_.jacob.block(0,3,6,2);
- MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
// task jacobian
JVS = Jqa1;
// task jacobian pseudo inverse
- // JVS_pseudo = calcPinv(Jqa1);
+ //JVS_pseudo = calcPinv(Jqa1);
JVS_pseudo = weight_jacvs_inverse(Jqa1);
+ //std::cout << "No weighted pseudo inverse: @line:" << __LINE__ << std::endl;
+
// task velocity vector
sigmaVS = this->cam_vel_-Jqa2*this->ctrl_params_.v_rollpitch;
}
--
GitLab