From 65441a4895b6efee95e064a8f5b68981aaec2b0a Mon Sep 17 00:00:00 2001 From: roberto rossi <roberto.rossi@polimi.it> Date: Sat, 27 Aug 2016 13:49:08 +0200 Subject: [PATCH] Corrected matrix JACEEPOS, from robot arm to translation --- src/tasks/eepos.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp index 29c9d90..eb56644 100644 --- a/src/tasks/eepos.cpp +++ b/src/tasks/eepos.cpp @@ -111,8 +111,9 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M // Task Error ________________________________________________ // End-Effector Position Error + sigmaEEPOS.block(0,0,3,1) = eepos_des.block(0,0,3,1) - HT.cam_in_w.block(0,3,3,1); - + // End-Effector Orientation Error sigmaEEPOS.block(3,0,3,1) = eepos_des.block(3,0,3,1) - RToEulerContinuous(HT.cam_in_w.block(0,0,3,3),eepos_des.block(3,0,3,1)); @@ -151,8 +152,9 @@ 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); + + 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; JEEPOS_omw.block(0,6,6,arm.nj) = RRb_in_w * ArmJac; Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6); @@ -163,8 +165,8 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M JEEPOS.block(0,6,6,arm.nj) = omega2euldiff_in_w*JEEPOS_omw.block(0,6,6,arm.nj); // task jacobian pseudo inverse - JEEPOS_pseudo = UAM::CCommon_Fc::WeightInvJac(JEEPOS,vs_delta_gain,vs_alpha_min,sigmaEEPOS.block(0,0,3,1).norm()); - + //JEEPOS_pseudo = UAM::CCommon_Fc::WeightInvJac(JEEPOS,vs_delta_gain,vs_alpha_min,sigmaEEPOS.block(0,0,3,1).norm()); + JEEPOS_pseudo = JEEPOS.transpose(); // Return reduced Jacobians (without roll and pitch columns) JEEPOS_reduced = Eigen::MatrixXd::Zero(6,4+arm.nj); JEEPOS_reduced.block(0,0,6,3) = JEEPOS.block(0,0,6,3); -- GitLab