removed hardcoded EE waypoint's orientation

src/tasks/eepos.cpp

@@ -108,28 +108,18 @@ 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);
   Eigen::Matrix3d Re_in_b = HT.cam_in_baselink.block(0,0,3,3);
@@ -142,39 +132,39 @@ 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
 
   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;
 
+  // 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;
+  
   // 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 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();
   
-  JEEPOS = Omw2eul*JEEPOS_omw; 
+  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); 
 
-  JEEPOS.block(0,6+arm.nj-1,6,1) = -JEEPOS.block(0,6+arm.nj-1,6,1);
+  // TODO: WTF is this?
+  JEEPOS.col(5+arm.nj) = -JEEPOS.col(5+arm.nj);
 
   // task jacobian pseudo inverse
   // JEEPOS_pseudo = UAM::CCommon_Fc::CalcPinv(JEEPOS);