From 5d34aaec4893e2116ed8089bc479d816449984af Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Wed, 5 Aug 2015 20:35:54 +0200
Subject: [PATCH] Simulation working with IBVS (all kinton autonomy nodes). ROS
node tag v0.3. With weighted pseudo-inverse. No saturations. With CoG
secondary task. No joints limits secondary task.
---
src/quadarm_task_priority_ctrl.cpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 703a1f0..67a5bbb 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -428,6 +428,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
// 3rd column of rotation matrix
IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).inverse().block(0,2,3,1);
+ // IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).block(0,2,3,1);
// Origin of the link's frame r.t. base_link
link_origin.at(jj+1) = T_base_to_joint.at(jj+1).col(3);
// Distance to the middle of the link jj
@@ -472,7 +473,7 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
// Debug
// std::cout << "mass: " << this->arm_.mass << std::endl;
// std::cout << "arm_cg: " << arm_cg << std::endl;
- // std::cout << "cog: " << this->arm_cog_data_.arm_cog << std::endl;
+ //std::cout << "cog: " << this->arm_cog_data_.arm_cog << std::endl;
// Rotation between quadrotor body and inertial frames
// TODO: If attitude is estimated, then it could be roll and pitch
--
GitLab