diff --git a/src/kinematics.cpp b/src/kinematics.cpp index 7080b3310ef1fcc88d2b2a7f8ce81da116ca0c06..5f8ef38d8f0d6b58d1e7c193c0775f5706098db3 100644 --- a/src/kinematics.cpp +++ b/src/kinematics.cpp @@ -25,7 +25,7 @@ void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT, Eigen::MatrixXd Jb2i = QuadJac(quad.pos); // Inertial to body frames - Eigen::MatrixXd Ji2b = CCommon_Fc::CalcPinv(Jb2i); + Eigen::MatrixXd Ji2b = Jb2i.inverse(); // Jacobian from quadrotor body to camera Eigen::MatrixXd Jb2c = Eigen::MatrixXd::Zero(6,6); @@ -182,11 +182,15 @@ Eigen::MatrixXd CKinematics::ArmJac(UAM::CArm& arm) Eigen::MatrixXd Ja = Eigen::MatrixXd::Zero(6,arm.nj); - for (int ii=0; ii<6; ++ii) + for (unsigned int ii=0; ii<6; ++ii) { for (unsigned int jj=0; jj<arm.nj; ++jj) Ja(ii,jj) = arm.jacobian.data(ii,jj); } + + // TODO: Define the arm in order to avoid this. + Ja.col(arm.nj-1) = -Ja.col(arm.nj-1); + return Ja; } diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp index 42d373feddc49a649bb7a6f93e4152c915d854c4..9433c9e78087845f3073b7b2a74001ad643f3d79 100644 --- a/src/tasks/eepos.cpp +++ b/src/tasks/eepos.cpp @@ -119,7 +119,6 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M // Task Jacobian _____________________________________________ // Quadrotor - Eigen::Matrix3d Rb_in_w = HT.baselink_in_w.block(0,0,3,3); Eigen::Matrix3d Re_in_w = HT.cam_in_w.block(0,0,3,3); Eigen::Matrix3d Re_in_b = HT.cam_in_baselink.block(0,0,3,3); @@ -132,13 +131,6 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M Eigen::Matrix3d wTb_inv = wTb.inverse(); Eigen::Matrix3d wTe = UAM::CCommon_Fc::GetWronskian(euRe_in_w(0),euRe_in_w(1)); - - Eigen::MatrixXd JEEPOS_omw = Eigen::MatrixXd::Zero(6,6+arm.nj); - JEEPOS_omw.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3); - JEEPOS_omw.block(3,0,3,3) = Eigen::MatrixXd::Zero(3,3); - JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(pe_in_b))*wTb_inv; - JEEPOS_omw.block(3,3,3,3) = Re_in_w*Re_in_b.transpose()*wTb_inv; - // TODO: Remove // Eigen::Matrix3d Aux_Frame_Matrix1 = Eigen::Vector3d(1,-1,1).asDiagonal(); // Eigen::Matrix3d Aux_Frame_Matrix1 = Eigen::Matrix3d::Zero(); @@ -147,6 +139,12 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M // Aux_Frame_Matrix1(2,0) = 1.0; //JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(Aux_Frame_Matrix1*pe_in_b))*wTb_inv; + Eigen::MatrixXd JEEPOS_omw = Eigen::MatrixXd::Zero(6,6+arm.nj); + JEEPOS_omw.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3); + JEEPOS_omw.block(3,0,3,3) = Eigen::MatrixXd::Zero(3,3); + JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(pe_in_b))*wTb_inv; + JEEPOS_omw.block(3,3,3,3) = Re_in_w*Re_in_b.transpose()*wTb_inv; + // Arm Eigen::MatrixXd ArmJac = UAM::CKinematics::ArmJac(arm); @@ -156,18 +154,14 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M RRb_in_w.block(3,3,3,3) = Rb_in_w*Rarmb_in_b; JEEPOS_omw.block(0,6,6,arm.nj) = RRb_in_w * ArmJac; - Eigen::MatrixXd Angvel_in_w2euldiff_in_w = Eigen::MatrixXd::Zero(6,6); - Angvel_in_w2euldiff_in_w.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3); - Angvel_in_w2euldiff_in_w.block(3,3,3,3) = wTe*Re_in_w.transpose(); + Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6); + omega2euldiff_in_w.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3); + omega2euldiff_in_w.block(3,3,3,3) = wTe*Re_in_w.transpose(); - JEEPOS.block(0,0,6,6) = Angvel_in_w2euldiff_in_w*JEEPOS_omw.block(0,0,6,6); - JEEPOS.block(0,6,6,arm.nj) = Angvel_in_w2euldiff_in_w*JEEPOS_omw.block(0,6,6,arm.nj); - - // TODO: WTF is this? - JEEPOS.col(5+arm.nj) = -JEEPOS.col(5+arm.nj); + JEEPOS.block(0,0,6,6) = omega2euldiff_in_w*JEEPOS_omw.block(0,0,6,6); + 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::CalcPinv(JEEPOS); JEEPOS_pseudo = UAM::CCommon_Fc::WeightInvJac(JEEPOS,vs_delta_gain,vs_alpha_min,sigmaEEPOS.block(0,0,3,1).norm()); // Return reduced Jacobians (without roll and pitch columns)