eepos.cpp

// Own
#include "eepos.h"
#include "common_fc.h"
#include "kinematics.h"

// MISC
#include <iostream>      /* input, output operations */

namespace UAM {

CTaskEEPOS::CTaskEEPOS(){}

CTaskEEPOS::~CTaskEEPOS(){}

Eigen::Vector3d CTaskEEPOS::RToEulerMin(const Eigen::Matrix3d& Rot)
{
  Eigen::Vector3d ang;

  if (Rot(2,0)!=-1.0 && Rot(2,0)!=1.0)
  {
    double phi1, theta1, psi1;
    double phi2, theta2, psi2;

    theta1 = -asin(Rot(2,0));
    theta2 = 3.141592653589793 - theta1;
    psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1)); 
    psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2)); 
    phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1)); 
    phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2)); 

    // TODO: This statement can cause big discontinuities in the obtained angles when 
    // the two norms are similar.
    if (sqrt(std::pow(theta1,2)+std::pow(psi1,2)+std::pow(phi1,2)) < sqrt(std::pow(theta2,2)+std::pow(psi2,2)+std::pow(phi2,2)))
      ang << psi1, theta1, phi1;
    else
      ang << psi2, theta2, phi2; // ZYX Euler convention.
  }
  else
  {
    double phi, theta, psi;
    phi = 0.0;
    if (Rot(2,0)==-1.0)
    {
      theta = 3.141592653589793/2;
      psi = phi + atan2(Rot(0,1),Rot(0,2)); 
    }
    else
    {
      theta = -3.141592653589793/2;
      psi = -phi + atan2(-Rot(0,1),-Rot(0,2)); 
    }
    ang << phi, theta, psi;
  }

  return ang;
}

Eigen::Vector3d CTaskEEPOS::RToEulerContinuous(const Eigen::Matrix3d& Rot, const Eigen::Vector3d& rpy_old)
{
  Eigen::Vector3d ang;

  if (Rot(2,0)!=-1.0 && Rot(2,0)!=1.0)
  {
    double phi1, theta1, psi1;
    double phi2, theta2, psi2;

    Eigen::Vector3d ang1;
    Eigen::Vector3d ang2;

    theta1 = -asin(Rot(2,0));
    theta2 = 3.141592653589793 - theta1;
    psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1)); 
    psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2)); 
    phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1)); 
    phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2)); 

    ang1 << psi1, theta1, phi1;
    ang2 << psi2, theta2, phi2;

    if ( (rpy_old-ang1).norm() < (rpy_old-ang2).norm())
      ang = ang1;
    else
      ang = ang2; // ZYX Euler convention.
  }
  else
  {
    double phi, theta, psi;
    phi = 0.0;
    if (Rot(2,0)==-1.0)
    {
      theta = 3.141592653589793/2;
      psi = phi + atan2(Rot(0,1),Rot(0,2)); 
    }
    else
    {
      theta = -3.141592653589793/2;
      psi = -phi + atan2(-Rot(0,1),-Rot(0,2)); 
    }
    ang << phi, theta, psi;
  }
  return ang;
}

void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::MatrixXd& vs_delta_gain, const double& vs_alpha_min, const Eigen::MatrixXd& eepos_des, Eigen::MatrixXd& sigmaEEPOS, Eigen::MatrixXd& JEEPOS_reduced, Eigen::MatrixXd& JEEPOS_pseudo_reduced)
{
  // Initialize sizes
  sigmaEEPOS = Eigen::MatrixXd::Zero(6,1);
  Eigen::MatrixXd JEEPOS = Eigen::MatrixXd::Zero(6,6+arm.nj);
  Eigen::MatrixXd JEEPOS_pseudo = Eigen::MatrixXd::Zero(6+arm.nj,6);

  // 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));

  // 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);
  Eigen::Vector3d pe_in_b = HT.cam_in_baselink.block(0,3,3,1);

  Eigen::Vector3d euRb_in_w = RToEulerMin(Rb_in_w);
  Eigen::Vector3d euRe_in_w = RToEulerMin(Re_in_w);
  
  Eigen::Matrix3d wTb = UAM::CCommon_Fc::GetWronskian(euRb_in_w(0),euRb_in_w(1));
  Eigen::Matrix3d wTb_inv = wTb.inverse();
  Eigen::Matrix3d wTe = UAM::CCommon_Fc::GetWronskian(euRe_in_w(0),euRe_in_w(1));

  // 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);
  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);

  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);

  // 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);
  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) = 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::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);
  JEEPOS_reduced.block(0,3,6,1+arm.nj) = JEEPOS.block(0,5,6,1+arm.nj);
  JEEPOS_pseudo_reduced = Eigen::MatrixXd::Zero(4+arm.nj,6);
  JEEPOS_pseudo_reduced.block(0,0,3,6) = JEEPOS_pseudo.block(0,0,3,6);
  JEEPOS_pseudo_reduced.block(3,0,1+arm.nj,6) = JEEPOS_pseudo.block(5,0,1+arm.nj,6);
}

} // End of UAM namespace