common_fc.cpp

#include "common_fc.h"

namespace UAM {

CCommon_Fc::CCommon_Fc(){}

CCommon_Fc::~CCommon_Fc(){}

Eigen::MatrixXd CCommon_Fc::CalcPinv(const Eigen::MatrixXd& a, double epsilon)
{
  Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
  double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0);
  Eigen::MatrixXd mpinv = svdd.matrixV() *  (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint();
  return mpinv;
}


Eigen::MatrixXd CCommon_Fc::WeightInvJac(const Eigen::MatrixXd& J, const Eigen::MatrixXd& invjac_delta_gain, const double& invjac_alpha_min, const double& target_dist)
{
  // W: Weighting matrix (motion distribution) ______________________
  double w_min = invjac_delta_gain(0,0);
  double w_max = invjac_delta_gain(1,0);
  double alpha_min = invjac_alpha_min;

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

  int numjoints = J.cols();
  Eigen::MatrixXd W = Eigen::MatrixXd::Zero(numjoints,numjoints);

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

  // weighting matrix
  Eigen::MatrixXd Winv = CalcPinv(W);
  Eigen::MatrixXd temp = J*Winv*J.transpose();
  Eigen::MatrixXd J_inverse = Winv*J.transpose()*CalcPinv(temp);
 
  return J_inverse;
}

Eigen::Matrix4d CCommon_Fc::GetTransform(const KDL::Frame& f)
{
  double yaw,pitch,roll;
  Eigen::Matrix3d R;
  Eigen::Matrix4d T;

  // f.M.GetEulerZYX(yaw,pitch,roll);
  f.M.GetRPY(roll,pitch,yaw);

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

  return T;
}

Eigen::Matrix4d CCommon_Fc::GetTransform(const Eigen::VectorXd& vec)
{
  assert(vec.size() == 6 && "Could not get transform. Vector must be of size 6 (x,y,z,ro,pi,ya)");

  Eigen::Matrix4d T;

  T(0,3) = vec(0);
  T(1,3) = vec(1);
  T(2,3) = vec(2);

  // euler convention zyx
  Eigen::Matrix3d R;
  R = Eigen::AngleAxisd(vec(5), Eigen::Vector3d::UnitZ()) \
    * Eigen::AngleAxisd(vec(4), Eigen::Vector3d::UnitY()) \
    * Eigen::AngleAxisd(vec(3), Eigen::Vector3d::UnitX());
  T.block(0,0,3,3)=R;
  T.row(3) << 0, 0, 0, 1;

  return T;
}

Eigen::Matrix3d CCommon_Fc::GetWronskian(const double& phi, const double& theta)
{
  Eigen::Matrix3d W;

  double pi = 3.14159265359;
  double eps = 1e-2;
  double mod = fmod(theta-pi/2,pi);
  double angth = theta;

  if (std::abs(mod) < eps)
  {
    if (angth > 0.0)
      angth = pi/2+eps;
    else
      angth = -pi/2+eps;
  }
  if ((pi-eps) < std::abs(mod) )
  {
    if (angth > 0.0)
      angth = pi/2-eps;
    else
      angth = -pi/2-eps;
  }

  W(0,0) = 1;
  W(0,1) = sin(phi)*tan(angth);
  W(0,2) = cos(phi)*tan(angth);
  W(1,0) = 0;
  W(1,1) = cos(phi);
  W(1,2) = -sin(phi);
  W(2,0) = 0;
  W(2,1) = sin(phi)/cos(angth);
  W(2,2) = cos(phi)/cos(angth);

  return W;
}

Eigen::Matrix3d CCommon_Fc::Skew(const Eigen::Vector3d& vec)
{
  Eigen::Matrix3d skew = Eigen::Matrix3d::Zero();
  skew(0,1) = -vec(2);
  skew(0,2) = vec(1);
  skew(1,0) = vec(2);
  skew(1,2) = -vec(0);
  skew(2,0) = -vec(1);
  skew(2,1) = vec(0);
  return skew;
}

std::string CCommon_Fc::get_datetime()
{
  time_t rawtime;
  struct tm * timeinfo;
  char buffer[80];

  time (&rawtime);
  timeinfo = localtime(&rawtime);

  // strftime(buffer,80,"%d%m%Y_%I%M%S",timeinfo);
  strftime(buffer,80,"%d%m%Y_%I%M",timeinfo);
  std::string str(buffer);

  return str;
}

void CCommon_Fc::write_to_file(const std::string& folder_name, const std::string& file_name, const Eigen::MatrixXd& data, const double& ts)
{
  // Get home directory
  int myuid;
  passwd *mypasswd;
  std::string path;
  myuid = getuid();
  mypasswd = getpwuid(myuid);
  path= mypasswd->pw_dir;

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

  // Add Folder with date and time
  // path+="/";
  // path+=this->datetime_;

  // Check if path exists, and create folder if needed
  struct stat sb;
  if (stat(path.c_str(), &sb) != 0)
  {
    mkdir(path.c_str(), S_IRWXU|S_IRGRP|S_IXGRP);
    std::string command = "mkdir -p ";
    std::stringstream ss;
    ss << command << path;
    std::string com = ss.str();
    int ret = system(com.c_str());
    if (WEXITSTATUS(ret)!=0)
      std::cout << "Error creating directory." << std::endl;
  }

  path+="/";

  // Create file
  std::string FileName;
  FileName = path;
  FileName+= file_name;
  std::ofstream File;
  File.open(FileName.c_str(), std::ios::out | std::ios::app | std::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();
}

} // End of UAM namespace