diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp index 29c9d90f609ae0670857a381c3780b97e2654d29..2118c9f9065ac9918272d5446e9324b22fa39f44 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);