From b780319ec30d9159937c7b5633c883bd7a09df04 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Sun, 28 Aug 2016 08:59:08 +0200
Subject: [PATCH] modified JEEPOS to have different RRb_in_w diagonal blocks. A
 TODO comment was added

---
 src/tasks/eepos.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp
index 29c9d90..2118c9f 100644
--- a/src/tasks/eepos.cpp
+++ b/src/tasks/eepos.cpp
@@ -151,8 +151,11 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   Eigen::MatrixXd RRb_in_w = Eigen::MatrixXd::Zero(6,6);
   Eigen::Matrix3d Rarmb_in_b = HT.armbase_in_baselink.block(0,0,3,3);
   Eigen::Matrix3d Rl1_in_armb = HT.link1_in_armbase.block(0,0,3,3);
-  RRb_in_w.block(0,0,3,3) = Rb_in_w * Rarmb_in_b * Rl1_in_armb;
-  RRb_in_w.block(3,3,3,3) = RRb_in_w.block(0,0,3,3);
+  // TODO: resolve why these blocks are different
+  RRb_in_w.block(0,0,3,3) =  Rb_in_w;
+  RRb_in_w.block(3,3,3,3) =  Rb_in_w * Rarmb_in_b * Rl1_in_armb;
+  // RRb_in_w.block(0,0,3,3) = Rb_in_w * Rarmb_in_b * Rl1_in_armb;
+  // RRb_in_w.block(3,3,3,3) = RRb_in_w.block(0,0,3,3);
   JEEPOS_omw.block(0,6,6,arm.nj) = RRb_in_w * ArmJac;
 
   Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6);
-- 
GitLab