quadarm_task_priority_ctrl.cpp

#include "quadarm_task_priority_ctrl.h"

using namespace Eigen;
using namespace KDL;
using namespace std;

CQuadarm_Task_Priority_Ctrl::CQuadarm_Task_Priority_Ctrl()
{
  // time (&this->time_ini_);
}
 
CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
{
}

// Main public function
void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_height,
                                                             const goal_obj& goal,
                                                             const ht& HT,
                                                             const arm& arm,
                                                             ctrl_params& ctrl_params,
																														 MatrixXd& joint_pos,
                                                             MatrixXd& robot_vel)
{
	
  // Current quadrotor height
  this->quad_height_ = quad_height;

  // Goal related objects
  this->vel_.cam = goal.cam_vel;
  this->target_dist_ = goal.target_dist;
  // Homogeneous transformations
  this->T_ = HT;
  // Control parameters
  this->ctrl_params_ = ctrl_params;
  // Arm data
  this->arm_.chain = arm.chain;
  this->arm_.joint_info= arm.joint_info;
  this->arm_.jnt_pos = arm.jnt_pos;

  // Arm DOFs
  this->arm_.nj = this->arm_.chain.getNrOfJoints();

  qa_kinematics();
  qa_control();

  // Arm positions increment
  this->arm_.jnt_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1)+(this->vel_.quadarm.block(6,0,this->arm_.nj,1) * this->ctrl_params_.dt);
  // Quadrotor positions increment
  this->arm_.jnt_pos.block(0,0,6,1) = this->arm_.jnt_pos.block(0,0,6,1)+(this->vel_.quadarm.block(0,0,6,1) * this->ctrl_params_.dt);

  // return values
  ctrl_params = this->ctrl_params_;
  joint_pos = this->arm_.jnt_pos;
  robot_vel = this->vel_.quadarm;
}

// Kinematics
void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){

  // Get arm Homogenous transform, arm tip in base_link
  this->T_.tip_in_baselink = arm_f_kinematics();
  
  // Update Transforms
  this->T_.cam_in_baselink = this->T_.tip_in_baselink * this->T_.cam_in_tip;
  this->T_.tag_in_baselink = this->T_.cam_in_baselink * this->T_.tag_in_cam;

  // Arm Jacobian 
  MatrixXd Ja = arm_jacobian();

  // Quadrotor Jacobian body to inertial frames
  MatrixXd Jb2i = quadrotor_jacobian();

  // Inertial to body frames
  MatrixXd Ji2b = calcPinv(Jb2i);

  // Jacobian from quadrotor body to camera
  Matrix3d skew = Matrix3d::Zero();
  skew(0,1)=this->T_.cam_in_baselink(2,3);
  skew(0,2)=-this->T_.cam_in_baselink(1,3);
  skew(1,0)=-this->T_.cam_in_baselink(2,3);
  skew(1,2)=this->T_.cam_in_baselink(0,3);
  skew(2,0)=this->T_.cam_in_baselink(1,3);
  skew(2,1)=-this->T_.cam_in_baselink(0,3);
  MatrixXd Jb2c = MatrixXd::Zero(6,6);
  Jb2c.block(0,0,3,3) = Matrix3d::Identity();
  Jb2c.block(0,3,3,3) = skew;
  Jb2c.block(3,3,3,3) = Matrix3d::Identity();

  // Rotation of camera in body frame
  Matrix3d Rc_in_b = this->T_.cam_in_baselink.block(0,0,3,3);
  // Rotation of body in camera frame
  Matrix3d Rb_in_c = Rc_in_b.transpose();

  // Transform Jacobian from body to camera frame
  MatrixXd Rot = MatrixXd::Zero(6,6);
  Rot.block(0,0,3,3)=Rb_in_c;
  Rot.block(3,3,3,3)=Rb_in_c;

  // Whole robot Jacobian
  this->Jqa_ = MatrixXd::Zero(6,6+this->arm_.nj);

  this->Jqa_.block(0,0,6,6) = Rot*Jb2c*Ji2b;
  this->Jqa_.block(0,6,6,this->arm_.nj)=Rot*Ja;
}
Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics()
{

  // constructs the kdl solver in non-realtime
  this->arm_.jnt_to_pose_solver.reset(new KDL::ChainFkSolverPos_recursive(this->arm_.chain));

  // Create joint array
  this->arm_.jnt_pos_kdl = JntArray(this->arm_.nj);

  // resize the joint state vector in non-realtime
  this->arm_.jnt_pos_kdl.resize(this->arm_.nj);

  for(unsigned int i=0; i < this->arm_.nj; i++){
    this->arm_.jnt_pos_kdl.data(i) = this->arm_.jnt_pos(6+i,0);
  }

  // computes Cartesian pose in realtime From Base_link to Arm tip
  Frame kdlTarm;
  this->arm_.jnt_to_pose_solver->JntToCart(this->arm_.jnt_pos_kdl, kdlTarm);
    
  double roll,pitch,yaw;
  kdlTarm.M.GetEulerZYX(yaw,pitch,roll);
    
  Matrix4d Tarm;
  Matrix3d Rarm;
  // euler convention zyx    
  Rarm = AngleAxisd(yaw, Vector3d::UnitZ()) \
       * AngleAxisd(pitch, Vector3d::UnitY()) \
     * AngleAxisd(roll, Vector3d::UnitX());
 
  Tarm.block(0,0,3,3) = Rarm;
  Tarm(0,3) = (double)kdlTarm.p.data[0];
  Tarm(1,3) = (double)kdlTarm.p.data[1];
  Tarm(2,3) = (double)kdlTarm.p.data[2];
  Tarm.row(3) << 0, 0, 0, 1;

  return Tarm;
}
MatrixXd CQuadarm_Task_Priority_Ctrl::arm_jacobian()
{
  // constructs the kdl solver in non-realtime
  this->arm_.jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(this->arm_.chain));

  // resize the joint state vector in non-realtime
  this->arm_.jacobian.resize(this->arm_.nj);

  // compute Jacobian in realtime
  this->arm_.jnt_to_jac_solver->JntToJac(this->arm_.jnt_pos_kdl, this->arm_.jacobian);

  MatrixXd Ja = MatrixXd::Zero(6,this->arm_.nj);

  for (int ii=0; ii<6; ++ii){
    for (unsigned int jj=0; jj<this->arm_.nj; ++jj){
      Ja(ii,jj) = this->arm_.jacobian.data(ii,jj);
    }
  }

  return Ja;
}
MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
{
  double psi,theta,phi;

  phi = this->arm_.jnt_pos(3,0);
  theta = this->arm_.jnt_pos(4,0);
  // psi = this->arm_.jnt_pos(5,0);
  psi = 0;

  MatrixXd Jq(6,6);
  Jq(0,0) = cos(psi)*cos(theta);
  Jq(0,1) = sin(phi)*cos(psi)*sin(theta)-cos(phi)*sin(psi);
  Jq(0,2) = cos(phi)*cos(psi)*sin(theta)+sin(phi)*sin(psi);
  Jq(1,0) = sin(psi)*cos(theta);
  Jq(1,1) = sin(phi)*sin(psi)*sin(theta)+cos(phi)*cos(psi);

  Jq(1,2) = cos(phi)*sin(psi)*sin(theta)-sin(phi)*cos(psi);
  Jq(2,0) = -sin(theta);
  Jq(2,1) = sin(phi)*cos(theta);
  Jq(2,2) = cos(phi)*cos(theta);
  Jq.block(0,3,3,3) = MatrixXd::Zero(3,3);
  Jq.block(3,0,3,3) = MatrixXd::Zero(3,3);
  Jq(3,3) = 1;
  Jq(3,4) = sin(phi)*tan(theta);
  Jq(3,5) = cos(phi)*tan(theta);
  Jq(4,3) = 0;
  Jq(4,4) = cos(phi);
  Jq(4,5) = -sin(phi);
  Jq(5,3) = 0;
  Jq(5,4) = sin(phi)*cos(theta);
  Jq(5,5) = cos(phi)*cos(theta);

  return Jq;
}

// Control
void CQuadarm_Task_Priority_Ctrl::qa_control(){
  
  // Control law ________________________________________________________________

  // TASK 0: Security task (Inflation radius) _____________________

  // task Jacobian and sigma
  MatrixXd JIR,JIR_pseudo,sigmaIR;
  task_infrad(JIR,JIR_pseudo,sigmaIR);
  
  // task velocity
  this->ctrl_params_.ir_vel = JIR_pseudo * sigmaIR;

  // TASK 1: Visual Servo _________________________________________
  
  // Null space projector
  MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
  MatrixXd Nqa1 = (eye-(JIR_pseudo*JIR));

  // task Jacobian and sigma
  MatrixXd JVS,JVS_wpseudo,sigmaVS;
  task_vs(JVS,JVS_wpseudo,sigmaVS);

  // task velocity
  this->ctrl_params_.vs_vel = Nqa1 * JVS_wpseudo * sigmaVS;

  // TASK 2: CoG alignement ______________________________________

  // Null space projector
  //MatrixXd Nqa1 = (eye-(JVS_wpseudo*JVS));
  MatrixXd JIR_VS = MatrixXd::Zero(JIR.rows()+JVS.rows(),JVS.cols());
  JIR_VS.block(0,0,JIR.rows(),JIR.cols()) = JIR;
  JIR_VS.block(JIR.rows()-1,0,JVS.rows(),JVS.cols())=JVS; 
  MatrixXd NIR_VS = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
  MatrixXd JIR_VS_pseudo = calcPinv(JIR_VS);
  NIR_VS = (eye-(JIR_VS_pseudo*JIR_VS));

  // task Jacobian and sigma
  MatrixXd JG,JG_pseudo,sigmaG;
  task_cog(JG,JG_pseudo,sigmaG);
  
  // Gain
  double lambdaG = this->ctrl_params_.cog_gain;
  
  // task velocity
  // this->ctrl_params_.cog_vel = Nqa1 * JG_pseudo * lambdaG * sigmaG;
  this->ctrl_params_.cog_vel = NIR_VS * JG_pseudo * lambdaG * sigmaG;

  // Third control task (Joint limits) ________________________
  
  // Augmented null space projector from Augmented Jacobian
  // MatrixXd JVS_G = MatrixXd::Zero(JVS.rows()+JG.rows(),JVS.cols());
  // JVS_G.block(0,0,JVS.rows(),JVS.cols()) = JVS;
  // JVS_G.block(JVS.rows()-1,0,JG.rows(),JG.cols())=JG; 
  // MatrixXd Nqa1_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
  // MatrixXd JVS_G_pseudo = calcPinv(JVS_G);
  
  MatrixXd JIR_VS_G = MatrixXd::Zero(JIR_VS.rows()+JG.rows(),JIR_VS.cols());
  JIR_VS_G.block(0,0,JIR_VS.rows(),JIR_VS.cols()) = JIR_VS;
  JIR_VS_G.block(JIR_VS.rows()-1,0,JG.rows(),JG.cols())=JG; 
  MatrixXd NIR_VS_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
  MatrixXd JIR_VS_G_pseudo = calcPinv(JIR_VS_G);


  // Third task after secondary aligning CoG
  //Nqa1_G = (eye-(JVS_G_pseudo*JVS_G));
  //Nqa1_G = Nqa1; // Third task as secondary
  NIR_VS_G = (eye-(JIR_VS_G_pseudo*JIR_VS_G));

  // task Jacobian and sigma
  MatrixXd JL,JL_pseudo,sigmaL;
  task_jl(JL,JL_pseudo,sigmaL);
  
  // Gain
  double lambdaL = this->ctrl_params_.jntlim_gain;

  // task velocity
  //this->ctrl_params_.jntlim_vel = Nqa1_G * JL_pseudo * lambdaL * sigmaL;
  this->ctrl_params_.jntlim_vel = NIR_VS_G * JL_pseudo * lambdaL * sigmaL;

  //******************************************************************
  // Weighted sum of subtasks
  // this->ctrl_params_.cog_vel = Nqa1 * (JG_pseudo * lambdaG * sigmaG + JL_pseudo * lambdaL * sigmaL);
  // this->ctrl_params_.jntlim_vel = MatrixXd::Zero(9,1);

  //******************************************************************
  //Total velocity __________________________________________
  if (!this->ctrl_params_.enable_sec_task) {
    this->ctrl_params_.cog_vel = MatrixXd::Zero(4+this->arm_.nj,1);
    this->ctrl_params_.jntlim_vel = MatrixXd::Zero(4+this->arm_.nj,1);
    cout << "[Task Priority Control]: Secondary tasks not enabled" << endl;
  }

  MatrixXd Vtotal(4+this->arm_.nj,1);
  //Vtotal = this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel; 
  //Vtotal = this->ctrl_params_.vs_vel + this->ctrl_params_.jntlim_vel; 
  Vtotal = this->ctrl_params_.ir_vel + this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel; 

  Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();

  // Check a singular configuration /////
  if (isnan(Vtotal(0,0)))
  {
    Vtotal=MatrixXd::Zero(4+this->arm_.nj,1);
  } 

  // Rearranging terms
  this->vel_.quadarm = MatrixXd::Zero(6+this->arm_.nj,1);
  this->vel_.quadarm.block(0,0,3,1) = Vtotal.block(0,0,3,1);
  this->vel_.quadarm.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1);
}
void CQuadarm_Task_Priority_Ctrl::task_infrad(MatrixXd& JIR,MatrixXd& JIR_pseudo,MatrixXd& sigmaIR)
{
  // Distance to the obstacle (currently detecting only the ground)
  MatrixXd d_obs = MatrixXd::Zero(3,1);
  d_obs(2,0) = this->quad_height_;

  // Inflation radius
  MatrixXd inf_radius = MatrixXd::Zero(1,1);
  inf_radius(0,0) = this->ctrl_params_.inf_radius;

  // Task vector 
  sigmaIR = d_obs.transpose()*d_obs-inf_radius;

  // Task Jacobian
  JIR = MatrixXd::Zero(1,4+this->arm_.nj);
  MatrixXd Jtemp = d_obs;
  JIR.block(0,0,1,3) = 2*Jtemp.transpose();

  // Bloquing matrix
  MatrixXd H = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);

  for (unsigned int ii = 0; ii < 3; ++ii)
    if (d_obs(ii,0) < inf_radius(0,0) && d_obs(ii,0) > 0.0)
    {
      cout << "WARNING!! Inflation Radius violation. DOF: " << ii << " Inf. radius: " << inf_radius(0,0) << " Obstacle distance: " << d_obs(ii,0) << endl;
      H(ii,ii) = 1.0;
    }

  MatrixXd Hinv = calcPinv(H);

  // weighting matrix
  MatrixXd temp = JIR*Hinv*JIR.transpose();
  JIR_pseudo = Hinv*JIR.transpose()*calcPinv(temp);

  // cout << "xxxxxx Task velocities xxxxxx" << endl;
  // cout << JIR_pseudo*sigmaIR << endl;
  // cout << "______ Task Jacobian _____" << endl;
  // cout << JIR << endl;
  // cout << "_______ Pseudo Inverse of Task Jacobian ____" << endl;
  // cout << JIR_pseudo << endl;
  // cout << "+++++++++" << endl;
}
void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,MatrixXd& sigmaVS)
{
  // Underactuated Quadrotor 
  MatrixXd J1(8,4),J2(8,2),Jqa1(6,4+this->arm_.nj),Jqa2(6,2);

  // 9 DOF Extracting wx and wy from quadrotor 
  Jqa1.block(0,0,6,3) = this->Jqa_.block(0,0,6,3);
  Jqa1.block(0,3,6,1+this->arm_.nj) = this->Jqa_.block(0,5,6,1+this->arm_.nj);
  Jqa2 = this->Jqa_.block(0,3,6,2);
  MatrixXd eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);

  // task jacobian
  JVS = Jqa1;

  // task jacobian pseudo inverse 
  //JVS_pseudo = calcPinv(Jqa1);
  JVS_pseudo = weight_jacvs_inverse(Jqa1);

  // task velocity vector
  sigmaVS = this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch;
}
void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,MatrixXd& sigmaG)
{
  // Initializations
  MatrixXd qa; // joint positions
  this->arm_.mass = 0; // Total arm mass
  MatrixXd mass = MatrixXd::Zero(this->arm_.nj,1); // Mass of the links
  this->ctrl_params_.cog_arm = MatrixXd::Zero(3,1); // Arm CoG
  MatrixXd arm_cg = MatrixXd::Zero(4,1); // Sum of CoG vector
  vector<Frame> joint_pose(this->arm_.nj); // Vector of Joint frames
  joint_pose.reserve(this->arm_.nj);
  vector<Matrix4d> Transf(this->arm_.nj); // Vector of HT of frames relative to each links
  Transf.reserve(this->arm_.nj);
  vector<MatrixXd> T_base_to_joint(this->arm_.nj+1); // Vector of HT of frame links relative to base
  T_base_to_joint.reserve(this->arm_.nj+1);
  vector<MatrixXd> IIIrdcolRot_b_x(this->arm_.nj+1); // Vector of r3d column of HT
  IIIrdcolRot_b_x.reserve(this->arm_.nj+1);
  vector<MatrixXd> frame_link(this->arm_.nj+1); // Origin of each link's frame
  frame_link.reserve(this->arm_.nj+1);

  vector<MatrixXd> dist(this->arm_.nj); // Vector of relative distance of each link to its CoGlink
  dist.reserve(this->arm_.nj);
  vector<MatrixXd> link_cg(this->arm_.nj); // Vector of link CoG relative to base
  link_cg.reserve(this->arm_.nj);

  // Get arm segments
  unsigned int js = this->arm_.chain.getNrOfSegments();
  vector<Segment> arm_segment(js);
  arm_segment.reserve(js);
  for (unsigned int ii = 1; ii < js; ++ii)
    arm_segment.at(ii) = this->arm_.chain.getSegment(ii);

  // Get joint positions
  qa = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);

  //Fixed transform from arm_base to base_link
  // Matrix4d Tbase = Matrix4d::Zero();
  // Tbase(0,0) = -1;
  // Tbase(1,1) = 1;
  // Tbase(2,2) = -1;
  // Tbase(3,3) = 1;
  Matrix4d Tbase = Matrix4d::Identity();

  // Origin of the base frame
  MatrixXd pini(4,1);
  pini.col(0) << 0,0,0,1;

  // Initial transform
  T_base_to_joint.at(0) = Tbase;
  // 3rd column of rotation matrix
  IIIrdcolRot_b_x.at(0) = T_base_to_joint.at(0).block(0,2,3,1);
  // Origin of the initial frame
  frame_link.at(0) = T_base_to_joint.at(0) * pini;

  // Debug
  //std::cout << "**************" << std::endl;
  this->arm_cog_data_.link.resize(this->arm_.nj);

  for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
  {
    // Get mass of each link
    mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass();
    // Joint frames  
    joint_pose.at(jj) = arm_segment.at(jj+1).pose(qa(jj,0));
    // relative Homogeneous transforms (HT)
    Transf.at(jj) = GetTransform(joint_pose.at(jj));
    // HT r.t. base link
    T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
    // 3rd column of rotation matrix
    IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).block(0,2,3,1);
    // Origin of the link's frame r.t. base_link
    frame_link.at(jj+1) = T_base_to_joint.at(jj+1) * pini;
    // Distance to the middle of the link jj
    dist.at(jj) = (frame_link.at(jj+1)-frame_link.at(jj))/2;
    // gravity for the link jj 
    link_cg.at(jj) = mass(jj,0)*(frame_link.at(jj)+dist.at(jj));
    // Sum of link CoGs
    arm_cg = arm_cg + link_cg.at(jj);
    // Sum of link mass  
    this->arm_.mass = this->arm_.mass + mass(jj,0);

    //Debug
    // std::cout << "num segments: " << arm_segment.size() << std::endl;
    // std::cout << "Link: " << jj << " name:";
    // std::cout << arm_segment.at(jj+1).getName();
    // std::cout << " mass: " << mass(jj,0);
    // std::cout << " link_cog:" << std::endl;
    // std::cout << frame_link.at(jj)+dist.at(jj)  << std::endl;
    // std::cout << "Transform: " << std::endl;
    // std::cout << T_base_to_joint.at(jj+1) <<std::endl;

    // Fill link information
    this->arm_cog_data_.link.at(jj).origin = T_base_to_joint.at(jj+1);
    this->arm_cog_data_.link.at(jj).cog = (frame_link.at(jj+1)-frame_link.at(jj))/2;
    this->arm_cog_data_.link.at(jj).mass = mass(jj,0);
  }
  // Debug
  // std::cout << "mass: " << this->arm_.mass << std::endl;
  // std::cout << "arm_cg: " << arm_cg << std::endl;

  // vector from arm base to CoG
  this->ctrl_params_.cog_arm = arm_cg/this->arm_.mass;

  // Debug
  // std::cout << "cog: " << this->ctrl_params_.cog_arm << std::endl;

  // Fill arm CoG information
  this->arm_cog_data_.arm_mass = this->arm_.mass;
  this->arm_cog_data_.arm_cog = this->ctrl_params_.cog_arm;


  // Rotation between quadrotor body and inertial frames  
  Matrix3d Rib;
  // Rib = AngleAxisd(0, Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitX());
  Rib = MatrixXd::Identity(3,3);

  // X and Y values of CoG in quadrotor inertial frame
  MatrixXd cog_arm_in_qi = Rib * this->ctrl_params_.cog_arm.block(0,0,3,1);

  // This change is done considering arm base frame orientation
  // TODO: Work with quadrotor orientation
  //std::cout << "******" << std::endl << cog_arm_in_qi << std::endl;
  this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
  //this->ctrl_params_.cog_PGxy = MatrixXd::Zero(2,1);
  //this->ctrl_params_.cog_PGxy(0,0) = cog_arm_in_qi(2,0);
  //this->ctrl_params_.cog_PGxy(1,0) = 	cog_arm_in_qi(1,0);

  MatrixXd X_partial;

  // Partial jacobian of each link (augmented links)
  MatrixXd Jj_cog(3,this->arm_.nj);
  for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
  {
    double partial_mass = 0;
    MatrixXd partial_arm_cg = MatrixXd::Zero(4,1);

    for (unsigned int ii = jj; ii < this->arm_.nj; ++ii)
    {
      partial_mass = partial_mass + mass(ii,0);
      partial_arm_cg = partial_arm_cg + link_cg.at(ii);
    }

    if(partial_mass!=0){
      X_partial = partial_arm_cg/partial_mass;

      Matrix3d screw_rot=Matrix3d::Zero();
      screw_rot(0,1) = -IIIrdcolRot_b_x.at(jj)(2);
      screw_rot(0,2) = IIIrdcolRot_b_x.at(jj)(1);
      screw_rot(1,0) = IIIrdcolRot_b_x.at(jj)(2);
      screw_rot(0,2) = -IIIrdcolRot_b_x.at(jj)(0);
      screw_rot(2,0) = -IIIrdcolRot_b_x.at(jj)(1);
      screw_rot(2,1) = IIIrdcolRot_b_x.at(jj)(0);

      Jj_cog.col(jj) = (partial_mass/this->arm_.mass)*screw_rot*X_partial.block(0,0,3,1);
    }
    else Jj_cog.col(jj) = MatrixXd::Zero(3,1);
  }

  JG=MatrixXd::Zero(1,4+this->arm_.nj);
  JG.block(0,0,1,4)=MatrixXd::Zero(1,4);
  MatrixXd Jacob_temp(3,this->arm_.nj);
  for (unsigned int ii = 0; ii < this->arm_.nj; ++ii)
  {
    Jacob_temp.col(ii) = Rib * Jj_cog.col(ii);
  }

  JG.block(0,4,1,this->arm_.nj)=2*this->ctrl_params_.cog_PGxy.transpose()*Jacob_temp.block(0,0,2,this->arm_.nj);

  JG_pseudo = calcPinv(JG);

  sigmaG = -(this->ctrl_params_.cog_PGxy.transpose()*this->ctrl_params_.cog_PGxy);
}
void CQuadarm_Task_Priority_Ctrl::task_jl(MatrixXd& JL,MatrixXd& JL_pseudo,MatrixXd& sigmaL)
{
  // Get diagonal gain matrix
  MatrixXd AAL=MatrixXd::Zero(this->arm_.nj,this->arm_.nj);
  for (unsigned int ii = 0; ii < this->arm_.nj; ++ii)
  {
      double min,max;
      min=this->arm_.joint_info.at(ii).joint_min;
      max=this->arm_.joint_info.at(ii).joint_max;
      // middle joints
      // this->ctrl_params_.jntlim_pos_des(ii,0) = min+0.5*(max-min);
      AAL(ii,ii) = 1/((max-min)*(max-min));
  }

  // Get current arm joint errors
  this->ctrl_params_.jntlim_pos_error = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1)-this->ctrl_params_.jntlim_pos_des;

  // Task sigma
  sigmaL = this->ctrl_params_.jntlim_pos_error.transpose()*AAL*this->ctrl_params_.jntlim_pos_error;

  // Task Jacobian
  JL = MatrixXd::Zero(1,4+this->arm_.nj);
  JL.block(0,0,1,4) = MatrixXd::Zero(1,4);
  MatrixXd Jtemp = AAL*this->ctrl_params_.jntlim_pos_error;
  JL.block(0,4,1,this->arm_.nj) = -2*Jtemp.transpose();

  JL_pseudo = calcPinv(JL);
}

// MISC functions
MatrixXd CQuadarm_Task_Priority_Ctrl::weight_jacvs_inverse(const MatrixXd& J)
{
  // W: Weighting matrix (motion distribution) ______________________
  double w_min = this->ctrl_params_.vs_delta_gain(0,0);
  double w_max = this->ctrl_params_.vs_delta_gain(1,0);
  double alpha_min = this->ctrl_params_.vs_alpha_min;

  double tmp = 2*3.14159*((this->target_dist_-w_min)/(w_max-w_min))-3.14159;
  double alpha = (0.5*(1+alpha_min)) + (0.5*(1-alpha_min))*tanh(tmp);

  MatrixXd W = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);

  // Create matrices
  for (unsigned int ii = 0; ii < 4+this->arm_.nj; ++ii)
  {
    // W Quadrotor values
    if(ii<4)
      W(ii,ii) = 1-alpha;
    // W Arm values
    else
      W(ii,ii) = alpha;
  }

  MatrixXd Winv = calcPinv(W);

  // weighting matrix
  MatrixXd temp = J*Winv*J.transpose();
  MatrixXd J_inverse = Winv*J.transpose()*calcPinv(temp);

  return J_inverse;
}
MatrixXd CQuadarm_Task_Priority_Ctrl::calcPinv(const MatrixXd &a){

  // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
  const MatrixXd* m;
  MatrixXd t;
  MatrixXd m_pinv;

  // transpose so SVD decomp can work...
  if ( a.rows()<a.cols() ){
          t = a.transpose();
          m = &t;
  } else {
          m = &a;
  }

  // SVD
  //JacobiSVD<MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
  JacobiSVD<MatrixXd> svd = m->jacobiSvd(ComputeThinU | ComputeThinV);
  MatrixXd vSingular = svd.singularValues();
  // Build a diagonal matrix with the Inverted Singular values
  // The pseudo inverted singular matrix is easy to compute :
  // is formed by replacing every nonzero entry by its reciprocal (inversing).
  MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1);
  for (int iRow =0; iRow<vSingular.rows(); iRow++) {
                                                   // Todo : Put epsilon in parameter
          if ( fabs(vSingular(iRow))<=1e-10 ) {
                  vPseudoInvertedSingular(iRow,0)=0.;
          }
          else {
                  vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
          }
  }
  // A little optimization here
  MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
  // Pseudo-Inversion : V * S * U'
  m_pinv = (svd.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * mAdjointU  ;

  // transpose back if necessary
  if ( a.rows()<a.cols() )
          return m_pinv.transpose();
  
  return m_pinv;
}
Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
{

  double yaw,pitch,roll;
  Matrix3d R;
  Matrix4d T;

  f.M.GetEulerZYX(yaw,pitch,roll);
  
  // euler convention zyx    
  R = Eigen::AngleAxisd(yaw, Vector3d::UnitZ()) \
    * Eigen::AngleAxisd(pitch, Vector3d::UnitY()) \
    * Eigen::AngleAxisd(roll, Vector3d::UnitX());
  T.block(0,0,3,3)=R;
  T(0,3) = (double)f.p.data[0];
  T(1,3) = (double)f.p.data[1];
  T(2,3) = (double)f.p.data[2];
  T.row(3) << 0, 0, 0, 1;

  return T;
}
void CQuadarm_Task_Priority_Ctrl::write_to_file(const string& folder_name, const string& file_name, const MatrixXd& data, const double& ts)
{
  // Get home directory
  int myuid;
  passwd *mypasswd;
  string path;
  myuid = getuid();
  mypasswd = getpwuid(myuid);
  path= mypasswd->pw_dir;
  // path+= "/Desktop/Results/";

  // Add Folder name
  stringstream folder;
  folder << folder_name;
  path+= folder.str();

  // mkdir(path.c_str(), S_IRWXU|S_IRGRP|S_IXGRP);
  // string command = "mkdir -p ";
  // stringstream ss;
  // ss << command << path;
  // string com = ss.str();
  // system(com.c_str());

  path+="/";

  // Create file
  string FileName;
  FileName = path;
  FileName+= file_name;
  ofstream File;
  File.open(FileName.c_str(), ios::out | ios::app | ios::binary);

  // Write time stamp
  File << ts;
  File << ",";

  // Write data in Matlab format
  for (int jj = 0; jj < data.cols(); ++jj)
  {
    for (int ii = 0; ii < data.rows()-1; ++ii)
    {
    File << data(ii,jj);
    File << ",";    
    }
  }
  // last row
  for (int jj = 0; jj < data.cols()-1; ++jj)
  {
    File << data(data.rows()-1,jj);
    File << ",";
  }
  // last term
  File << data(data.rows()-1,data.cols()-1);
  File << "\n";
  File.close();
}