diff --git a/src/kinematics.cpp b/src/kinematics.cpp
index 0ec0f0ed13d2132f4446ee998cb34a8149b460f7..5f8ef38d8f0d6b58d1e7c193c0775f5706098db3 100644
--- a/src/kinematics.cpp
+++ b/src/kinematics.cpp
@@ -25,7 +25,6 @@ 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
@@ -183,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/cog.cpp b/src/tasks/cog.cpp
index 496394e05eafbebd5a4f8364f3a9118ae346c04b..74b9dd8e7836052bba39b64f9bd1995f3f0079f7 100644
--- a/src/tasks/cog.cpp
+++ b/src/tasks/cog.cpp
@@ -170,7 +170,8 @@ void CTaskCOG::TaskErrorJacFull(UAM::CArm& arm, const UAM::CHT& HT, Eigen::Matri
JG = Eigen::MatrixXd::Zero(2,4+arm.nj);
JG.block(0,4,2,arm.nj) = Jacob_temp.block(0,0,2,arm.nj);
- JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
+ //JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
+ JG_pseudo = JG.transpose();
sigmaG = -cog_PGxy;
}
@@ -189,7 +190,8 @@ void CTaskCOG::TaskErrorJac(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd&
JG.block(0,4,1,arm.nj)=2*cog_PGxy.transpose()*Jacob_temp.block(0,0,2,arm.nj);
- JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
+ //JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
+ JG_pseudo = JG.transpose();
sigmaG = -(cog_PGxy.transpose()*cog_PGxy);
}
diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp
index faa1604508f25001901eda5be5c51276145a6d4d..9433c9e78087845f3073b7b2a74001ad643f3d79 100644
--- a/src/tasks/eepos.cpp
+++ b/src/tasks/eepos.cpp
@@ -108,27 +108,16 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
Eigen::MatrixXd JEEPOS = Eigen::MatrixXd::Zero(6,6+arm.nj);
Eigen::MatrixXd JEEPOS_pseudo = Eigen::MatrixXd::Zero(6+arm.nj,6);
- // End-Effector Position Error
+ // 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
-
- // TODO: extract
- Eigen::MatrixXd eepos_des_rot_DEBUG = Eigen::MatrixXd::Zero(3,1);
- eepos_des_rot_DEBUG << -1.5, 0.0, -1.5423;
-
- Eigen::Matrix3d curr_eeRot = HT.cam_in_w.block(0,0,3,3);
- Eigen::Matrix3d des_eeRot; // Desired Rotation (Euler ZYX)
- des_eeRot = Eigen::AngleAxisd(eepos_des(3,0), Eigen::Vector3d::UnitZ())
- * Eigen::AngleAxisd(eepos_des(4,0), Eigen::Vector3d::UnitY())
- * Eigen::AngleAxisd(eepos_des(5,0), Eigen::Vector3d::UnitX());
-
- Eigen::Matrix3d err_eeRot = curr_eeRot.transpose()*des_eeRot;
-
- // End-Effector Task Error
- sigmaEEPOS.block(3,0,3,1) = eepos_des_rot_DEBUG - RToEulerContinuous(curr_eeRot,eepos_des_rot_DEBUG);
-
- // Task Jacobian
+ 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));
+
+ // 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);
@@ -142,42 +131,37 @@ 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));
- // Quad linear part
-
+ // TODO: Remove
+ // Eigen::Matrix3d Aux_Frame_Matrix1 = Eigen::Vector3d(1,-1,1).asDiagonal();
+ // Eigen::Matrix3d Aux_Frame_Matrix1 = Eigen::Matrix3d::Zero();
+ // Aux_Frame_Matrix1(0,2) = 1.0;
+ // Aux_Frame_Matrix1(1,1) = -1.0;
+ // 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);
- Eigen::Matrix3d Aux_Frame_Matrix1 = Eigen::Vector3d(1,-1,1).asDiagonal();
- Eigen::Matrix3d Aux_Frame_Matrix2 = Eigen::Vector3d(-1, 1,-1).asDiagonal();
-
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(Aux_Frame_Matrix1*pe_in_b))*wTb_inv;
+ 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;
- //JEEPOS.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
- //JEEPOS.block(3,0,3,3) = Eigen::MatrixXd::Zero(3,3);
- // Quad angular part
- //JEEPOS.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(pe_in_b)*invwTb);
- //JEEPOS.block(3,3,3,3) = wTe*Re_in_b.transpose()*invwTb;
// Arm
Eigen::MatrixXd ArmJac = UAM::CKinematics::ArmJac(arm);
Eigen::MatrixXd RRb_in_w = Eigen::MatrixXd::Zero(6,6);
- RRb_in_w.block(0,0,3,3) = Rb_in_w*Aux_Frame_Matrix1;
- RRb_in_w.block(3,3,3,3) = Rb_in_w*Aux_Frame_Matrix2;
-// RRb_in_w.block(3,3,3,3) = wTe*Re_in_w.transpose()*Rb_in_w;
-
+ Eigen::Matrix3d Rarmb_in_b = Eigen::Vector3d(-1, 1,-1).asDiagonal();
+ RRb_in_w.block(0,0,3,3) = Rb_in_w*Rarmb_in_b;
+ 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 Omw2eul = Eigen::MatrixXd::Zero(6,6);
- Omw2eul.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
- Omw2eul.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 = Omw2eul*JEEPOS_omw;
-
- JEEPOS.block(0,6+arm.nj-1,6,1) = -JEEPOS.block(0,6+arm.nj-1,6,1);
+ 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)