jl.cpp

// Own
#include "jl.h"
#include <common_fc.h>

namespace UAM {

CTaskJL::CTaskJL(){}

CTaskJL::~CTaskJL(){}

Eigen::MatrixXd CTaskJL::TaskErrorCommon(const UAM::CArm& arm, const double& pow_n, const Eigen::MatrixXd& jntlim_pos_des, Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& AAL)
{
  AAL = Eigen::MatrixXd::Zero(arm.nj,arm.nj);
  for (unsigned int ii = 0; ii < arm.nj; ++ii)
  {
    // Get diagonal gain matrix
    double min,max;
    min = arm.joint_info.at(ii).joint_min;
    max = arm.joint_info.at(ii).joint_max;
    AAL(ii,ii) = 1/std::pow(max-min,pow_n);

    // Get joint position errors
    jntlim_pos_error = jntlim_pos_des-arm.jnt_pos;
  }
  return AAL;
}

void CTaskJL::TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des, Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo)
{
  // Get common elements
  double pow_n = 4;
  Eigen::MatrixXd AAL;
  TaskErrorCommon(arm,pow_n,jntlim_pos_des,jntlim_pos_error,AAL);

  // Task sigma
  sigmaL = jntlim_pos_error.transpose()*AAL*jntlim_pos_error;

  // Task Jacobian
  JL = Eigen::MatrixXd::Zero(1,4+arm.nj);
  JL.block(0,0,1,4) = Eigen::MatrixXd::Zero(1,4);
  for (unsigned int ii = 0; ii < jntlim_pos_error.rows(); ++ii)
    jntlim_pos_error(ii,0) = std::pow(jntlim_pos_error(ii,0),pow_n-1);
  Eigen::MatrixXd Jtemp = AAL*jntlim_pos_error;
  JL.block(0,4,1,arm.nj) = pow_n*Jtemp.transpose();

  // Task Jacobian Pseudo-inverse
  JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
}

void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des, Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo)
{
  // Get common elements
  double pow_n = 4;
  Eigen::MatrixXd AAL;
  TaskErrorCommon(arm,pow_n,jntlim_pos_des,jntlim_pos_error,AAL);

  sigmaL = Eigen::MatrixXd::Zero(arm.nj,1);
  JL = Eigen::MatrixXd::Zero(arm.nj,4+arm.nj);  
    
  for (unsigned int ii = 0; ii < arm.nj; ++ii)
  {
    // Task sigma
    sigmaL(ii,0) = AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n); 
    // Task Jacobian
    JL(ii,4+ii) = pow_n*AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n-1);
  }   

  // Task Jacobian Pseudo-inverse
  JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
}

} // End of UAM namespace