【机械臂】变量转换

robot_math_convert.h

#include <tf/transform_broadcaster.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include "geometry_msgs/Pose.h"

extern bool RM_Matrix_to_Pose1(Eigen::Matrix4d T, geometry_msgs::Pose *pose);
extern bool RM_Matrix_to_Pose(double* T, geometry_msgs::Pose *pose);
extern bool RM_Pose_to_Matrix1(geometry_msgs::Pose pose, Eigen::Matrix4d* T);
extern bool RM_Pose_to_Matrix(geometry_msgs::Pose pose, double* T);

extern bool RM_StampedTransform_to_Pose(tf::StampedTransform tarTrans, geometry_msgs::Pose *pose);
extern bool RM_Pose_to_StampedTransform(geometry_msgs::Pose pose, tf::StampedTransform *tarTrans);

extern void RM_eigenVectorXd_to_stdVector(Eigen::VectorXd eigV, std::vector<double> *stdV);
extern void RM_stdVector_to_eigenVectorXd(std::vector<double> stdV, Eigen::VectorXd *eigV);


extern geometry_msgs::Pose RM_Pose_left_Transform(geometry_msgs::Pose pose, tf::StampedTransform trans);
extern geometry_msgs::Pose RM_Pose_right_Transform(geometry_msgs::Pose pose, tf::StampedTransform trans);

robot_math_convert.cpp

#include "robot_math_convert.h"

#include <tf/transform_broadcaster.h>
#include "geometry_msgs/Pose.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
// ==============================================================================================================
// 基本数学工具
static double sign(double x);

bool RM_Matrix_to_Pose1(Eigen::Matrix4d T, geometry_msgs::Pose *pose);
bool RM_Matrix_to_Pose(double* T, geometry_msgs::Pose *pose);
bool RM_Pose_to_Matrix1(geometry_msgs::Pose pose, Eigen::Matrix4d* T);
bool RM_Pose_to_Matrix(geometry_msgs::Pose pose, double* T);

static double sign(double x)
{
    if (x >= 0.0)
    {
        return 1.0;
    }
    else
    {
        return -1.0;
    }
}

bool RM_Matrix_to_Pose1(Eigen::Matrix4d T, geometry_msgs::Pose *pose)
{
  // // Trans-Matrix
  // double r11 = T(0,0);
  // double r12 = T(0,1);
  // double r13 = T(0,2);
  // double r21 = T(1,0);
  // double r22 = T(1,1);
  // double r23 = T(1,2);
  // double r31 = T(2,0);
  // double r32 = T(2,1);
  // double r33 = T(2,2);

  // // Trans-Matrix to unit-quaternion
  // double q0 = 0.5*sqrt(r11+r22+r33+1);
  // double q1 = 0.5*sign(r32-r23)*sqrt(r11-r22-r33+1);
  // double q2 = 0.5*sign(r13-r31)*sqrt(r22-r33-r11+1);
  // double q3 = 0.5*sign(r21-r12)*sqrt(r33-r11-r22+1);

  // pose->position.x = T(0,3);
  // pose->position.y = T(1,3);
  // pose->position.z = T(2,3);
  // pose->orientation.w = q0;
  // pose->orientation.x = q1;
  // pose->orientation.y = q2;
  // pose->orientation.z = q3;


  Eigen::Vector4d  q;//q = numpy.empty((4, ), dtype=numpy.float64)
  Eigen::Matrix4d M = T;//  M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4)
  double  t = M.trace();
  if (t > M(3,3))
  {
      q(3) = t;
      q(2) = M(1, 0) - M(0, 1);
      q(1) = M(0, 2) - M(2, 0);
      q(0) = M(2, 1) - M(1, 2);
  }
  else
  {
      int i = 0;
      int j = 1;
      int k = 2; //0, 1, 2;
      if (M(1, 1) > M(0, 0))
      {
        i = 1;
        j = 2;
        k = 0; //1, 2, 0;
      }
          
      if (M(2, 2) > M(i, i))
      {
        i = 2;
        j = 0;
        k = 1; //2, 0, 1;
      }
          
      t = M(i, i) - (M(j, j) + M(k, k)) + M(3, 3);
      q(i) = t;
      q(j) = M(i, j) + M(j, i);
      q(k) = M(k, i) + M(i, k);
      q(3) = M(k, j) - M(j, k);
  }

  q *= 0.5 / sqrt(t * M(3, 3));
  pose->position.x = T(0,3);
  pose->position.y = T(1,3);
  pose->position.z = T(2,3);  
  pose->orientation.x = q(0);
  pose->orientation.y = q(1);
  pose->orientation.z = q(2);
  pose->orientation.w = q(3);

  // std::cout << "T" << std::endl;
  // std::cout << T << std::endl;
  // std::cout << "q0 << q1 << q2 << q3" << std::endl;
  // std::cout << q0 << "," << q1 << "," <<  q2 << "," <<  q3 << "," <<  std::endl;
  // std::cout << "q" << std::endl;
  // std::cout << q << std::endl;

  return true;
}

bool RM_Matrix_to_Pose(double* T, geometry_msgs::Pose *pose)
{
  // Trans-Matrix
  double r11 = T[0];
  double r12 = T[1];
  double r13 = T[2];
  double r21 = T[0+4];
  double r22 = T[1+4];
  double r23 = T[2+4];
  double r31 = T[0+8];
  double r32 = T[1+8];
  double r33 = T[2+8];

  // Trans-Matrix to unit-quaternion
  double q0 = 0.5*sqrt(r11+r22+r33+1);
  double q1 = 0.5*sign(r32-r23)*sqrt(r11-r22-r33+1);
  double q2 = 0.5*sign(r13-r31)*sqrt(r22-r33-r11+1);
  double q3 = 0.5*sign(r21-r12)*sqrt(r33-r11-r22+1);

  pose->position.x = T[3];
  pose->position.y = T[3+4];
  pose->position.z = T[3+8];
  pose->orientation.w = q0;
  pose->orientation.x = q1;
  pose->orientation.y = q2;
  pose->orientation.z = q3;

  return true;
}


bool RM_Pose_to_Matrix1(geometry_msgs::Pose pose, Eigen::Matrix4d* T)
{
    // trans
    double r14 = pose.position.x;
    double r24 = pose.position.y;
    double r34 = pose.position.z;

    // quaternion
    double q0 = pose.orientation.w;
    double q1 = pose.orientation.x;
    double q2 = pose.orientation.y;
    double q3 = pose.orientation.z;
    
    // Trans-Marix
    double r11 = 2.0 * (q0 * q0 + q1 * q1) - 1.0;
    double r12 = 2.0 * (q1 * q2 - q0 * q3);
    double r13 = 2.0 * (q1 * q3 + q0 * q2);
    double r21 = 2.0 * (q1 * q2 + q0 * q3);
    double r22 = 2.0 * (q0 * q0 + q2 * q2) - 1.0;
    double r23 = 2.0 * (q2 * q3 - q0 * q1);
    double r31 = 2.0 * (q1 * q3 - q0 * q2);
    double r32 = 2.0 * (q2 * q3 + q0 * q1);
    double r33 = 2.0 * (q0 * q0 + q3 * q3) - 1.0;

    (*T)(0,0) = r11;    (*T)(0,1) = r12;    (*T)(0,2) = r13;    (*T)(0,3) = r14;
    (*T)(1,0) = r21;    (*T)(1,1) = r22;    (*T)(1,2) = r23;    (*T)(1,3) = r24;
    (*T)(2,0) = r31;    (*T)(2,1) = r32;    (*T)(2,2) = r33;    (*T)(2,3) = r34;
    (*T)(3,0) = 0.0;    (*T)(3,1) = 0.0;    (*T)(3,2) = 0.0;    (*T)(3,3) = 1.0;

    return true;
}

bool RM_Pose_to_Matrix(geometry_msgs::Pose pose, double* T)
{
    // trans
    double r14 = pose.position.x;
    double r24 = pose.position.y;
    double r34 = pose.position.z;

    // quaternion
    double q0 = pose.orientation.w;
    double q1 = pose.orientation.x;
    double q2 = pose.orientation.y;
    double q3 = pose.orientation.z;
    
    // Trans-Marix
    double r11 = 2.0 * (q0 * q0 + q1 * q1) - 1.0;
    double r12 = 2.0 * (q1 * q2 - q0 * q3);
    double r13 = 2.0 * (q1 * q3 + q0 * q2);
    double r21 = 2.0 * (q1 * q2 + q0 * q3);
    double r22 = 2.0 * (q0 * q0 + q2 * q2) - 1.0;
    double r23 = 2.0 * (q2 * q3 - q0 * q1);
    double r31 = 2.0 * (q1 * q3 - q0 * q2);
    double r32 = 2.0 * (q2 * q3 + q0 * q1);
    double r33 = 2.0 * (q0 * q0 + q3 * q3) - 1.0;

    *T = r11; T++;
    *T = r12; T++;
    *T = r13; T++;
    *T = r14; T++;
    *T = r21; T++;
    *T = r22; T++;
    *T = r23; T++;
    *T = r24; T++;
    *T = r31; T++;
    *T = r32; T++;
    *T = r33; T++;
    *T = r34; T++;
    *T = 0.0; T++;
    *T = 0.0; T++;
    *T = 0.0; T++;
    *T = 1.0; T++;

    return true;
}

bool RM_StampedTransform_to_Pose(tf::StampedTransform tarTrans, geometry_msgs::Pose *pose)
{
  pose->position.x = tarTrans.getOrigin().x();
  pose->position.y = tarTrans.getOrigin().y();
  pose->position.z = tarTrans.getOrigin().z();
  pose->orientation.x = tarTrans.getRotation().x();
  pose->orientation.y = tarTrans.getRotation().y();
  pose->orientation.z = tarTrans.getRotation().z();
  pose->orientation.w = tarTrans.getRotation().w();
  return true;
}

bool RM_Pose_to_StampedTransform(geometry_msgs::Pose pose, tf::StampedTransform *tarTrans)
{
  tarTrans->setOrigin(tf::Vector3(pose.position.x, pose.position.y, pose.position.z));
  tarTrans->setRotation(tf::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w));
  return true;
}

void RM_eigenVectorXd_to_stdVector(Eigen::VectorXd eigV, std::vector<double> *stdV)
{
  for (int i = 0; i < stdV->size(); i++)
  {
    (*stdV)[i] = eigV(i);
  }
}

void RM_stdVector_to_eigenVectorXd(std::vector<double> stdV, Eigen::VectorXd *eigV)
{
  for (int i = 0; i < stdV.size(); i++)
  {
    (*eigV)(i) = stdV[i];
  }
}

geometry_msgs::Pose RM_Pose_right_Transform(geometry_msgs::Pose pose, tf::StampedTransform trans)
{
  tf::StampedTransform result_trans, start_trans;
  geometry_msgs::Pose result_pose;
  start_trans.setOrigin(tf::Vector3(pose.position.x, 
                                    pose.position.y, 
                                    pose.position.z));
  start_trans.setRotation(tf::Quaternion(pose.orientation.x, 
                                    pose.orientation.y, 
                                    pose.orientation.z, 
                                    pose.orientation.w)); 
  result_trans.mult(start_trans, trans);
  // result_trans = start_trans;
  // result_trans.operator*=(trans);

  pose.position.x = result_trans.getOrigin().x();
  pose.position.y = result_trans.getOrigin().y();
  pose.position.z = result_trans.getOrigin().z();
  pose.orientation.x = result_trans.getRotation().x();
  pose.orientation.y = result_trans.getRotation().y();
  pose.orientation.z = result_trans.getRotation().z();
  pose.orientation.w = result_trans.getRotation().w();

  return pose;
}

geometry_msgs::Pose RM_Pose_left_Transform(geometry_msgs::Pose pose, tf::StampedTransform trans)
{
  tf::StampedTransform result_trans, start_trans;
  geometry_msgs::Pose result_pose;
  start_trans.setOrigin(tf::Vector3(pose.position.x, 
                                    pose.position.y, 
                                    pose.position.z));
  start_trans.setRotation(tf::Quaternion(pose.orientation.x, 
                                    pose.orientation.y, 
                                    pose.orientation.z, 
                                    pose.orientation.w)); 
  result_trans.mult(trans, start_trans);

  pose.position.x = result_trans.getOrigin().x();
  pose.position.y = result_trans.getOrigin().y();
  pose.position.z = result_trans.getOrigin().z();
  pose.orientation.x = result_trans.getRotation().x();
  pose.orientation.y = result_trans.getRotation().y();
  pose.orientation.z = result_trans.getRotation().z();
  pose.orientation.w = result_trans.getRotation().w();

  return pose;
}