【机械臂】基于UR机械臂的运动学函数

最新推荐文章于 2024-07-02 22:40:29 发布

A MAN NAMED MAGIC

最新推荐文章于 2024-07-02 22:40:29 发布

阅读量614

点赞数 1

分类专栏：机械臂

本文链接：https://blog.csdn.net/weixin_43259286/article/details/109080605

版权

机械臂专栏收录该内容

3 篇文章 0 订阅

订阅专栏

这个代码实现了UR机器人（Universal Robots）的正向和逆向运动学，以及几何雅克比矩阵的计算。正向运动学包括多个函数，用于从关节角度到工具坐标系的转换。逆向运动学则提供了多种求解方法，包括无约束和约束条件下的解法。此外，还包括了寻找最近关节值和优化解的函数。整个库旨在为UR机器人的控制和路径规划提供数学基础。

摘要由CSDN通过智能技术生成

ur_kinematics_usr.h

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

#include <Eigen/Core>
#include <Eigen/Geometry>

// --------------------------------------------------------------------------------------------------------------
// 正解
extern void RM_forward(const double* q, double* T);
extern void RM_forward(Eigen::VectorXd joint, Eigen::Matrix4d* mat);
extern void RM_forward(Eigen::VectorXd joint, geometry_msgs::Pose* pose);
extern void RM_forward_all(const double* q, double* T1, double* T2, double* T3, 
                                  double* T4, double* T5, double* T6);

// --------------------------------------------------------------------------------------------------------------
// 逆解
extern int RM_inverse_limited(const geometry_msgs::Pose pose, std::vector<Eigen::VectorXd> *q_vector);
extern int RM_inverse_limited(const Eigen::Matrix4d T, std::vector<Eigen::VectorXd> *q_vector);
extern int RM_inverse_limited(const Eigen::MatrixXd T, std::vector<Eigen::VectorXd> *q_vector);
extern int RM_inverse(const Eigen::MatrixXd T, std::vector<Eigen::VectorXd> *q_vector);
extern int RM_inverse(const double* T, double* q_sols, double q6_des);
extern int RM_inverse_jointlimited(const double* T, double* q_sols, double q6_des);

extern int RM_inverse_constrain_shoulder_elbow_wrist(const geometry_msgs::Pose pose, std::vector<double> *q_vector, uint32_t constrain);
extern int RM_inverse_constrain_shoulder_elbow_wrist(const Eigen::Matrix4d T, std::vector<double> *q_vector, uint32_t constrain);
extern int RM_inverse_constrain_shoulder_elbow_wrist(const Eigen::MatrixXd T, std::vector<double> *q_vector, uint32_t constrain);
extern int RM_inverse_constrain_shoulder_elbow_wrist(const double* T, double* q_sols, uint32_t constrain);
// --------------------------------------------------------------------------------------------------------------
// 几何雅各比矩阵
extern void RM_Jacobian(const double* q, double* J);
extern void RM_JacobianMatrix_From_JointVector(const Eigen::VectorXd joint, Eigen::MatrixXd* Jacobian);

// --------------------------------------------------------------------------------------------------------------
// 寻找最近关节值
extern int RM_find_nearest_joint_index(std::vector<Eigen::VectorXd> q_vector_group, int slov_num, Eigen::VectorXd q_ref, Eigen::VectorXd* q_result);
extern int RM_find_nearest_joint_index(std::vector<Eigen::VectorXd> q_vector_group, int slov_num, Eigen::VectorXd* q_vector);
extern int RM_find_nearest_joint_index(double* q_sols, int slov_num, double* q_ref);

// --------------------------------------------------------------------------------------------------------------
// 逆解中寻优
extern bool RM_tarMat_nearJ_bestJ(double* trans, std::vector<double> nearJ, std::vector<double> *bestJ);
extern bool RM_tarPose_nearJ_bestJ(geometry_msgs::Pose tarPose, std::vector<double> nearJ, std::vector<double> *bestJ);
extern bool RM_tarTrans_nearJ_bestJ(tf::StampedTransform tarTrans, std::vector<double> nearJ, std::vector<double> *bestJ);
extern bool RM_Inverse_nearJ_bestJ(const Eigen::Matrix4d T, Eigen::VectorXd q_ref, Eigen::VectorXd *q_best);
extern bool RM_Inverse_nearJ_bestJ(const Eigen::MatrixXd T, Eigen::VectorXd q_ref, Eigen::VectorXd *q_best);
extern bool RM_Inverse_nearJ_bestJ(const geometry_msgs::Pose pose, Eigen::VectorXd q_ref, Eigen::VectorXd *q_best);

ur_kinematics_usr.cpp

#include "ur_kinematics_usr.h"
#include "robot_math_convert.h"
#include "robot_math_compute.h"

#include <tf/transform_broadcaster.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#define DEBUG_STD_OUT (0)

// ==============================================================================================================
// 基本数学工具
#define ZERO_THRESH 0.001
#define PI 3.1415926
int SIGN(double x);

// 关节角度限制
const double Jlimit_min[6] = {-PI, -PI, -PI, -PI, -PI, -PI};
const double Jlimit_max[6] = {PI, PI, PI, PI, PI, PI};

// 机械臂连杆参数
const double d1 =  0.1518;
const double a2 = -0.2435;
const double a3 = -0.213025;
const double d4 =  0.13095;
const double d5 =  0.0855;
const double d6 =  0.09195;
const double ltool = 0.185;

// ==============================================================================================================

// --------------------------------------------------------------------------------------------------------------
// 正解
void RM_forward(const double* q, double* T);
void RM_forward(Eigen::VectorXd joint, Eigen::Matrix4d* mat);
void RM_forward(Eigen::VectorXd joint, geometry_msgs::Pose* pose);
void RM_forward_all(const double* q, double* T1, double* T2, double* T3, 
                                  double* T4, double* T5, double* T6);

// --------------------------------------------------------------------------------------------------------------
// 逆解
int RM_inverse_limited(const geometry_msgs::Pose pose, std::vector<Eigen::VectorXd> *q_vector);
int RM_inverse_limited(const Eigen::Matrix4d T, std::vector<Eigen::VectorXd> *q_vector);
int RM_inverse_limited(const Eigen::MatrixXd T, std::vector<Eigen::VectorXd> *q_vector);
int RM_inverse(const Eigen::MatrixXd T, std::vector<Eigen::VectorXd> *q_vector);
int RM_inverse(const double* T, double* q_sols, double q6_des);
int RM_inverse_jointlimited(const double* T, double* q_sols, double q6_des);

int RM_inverse_constrain_shoulder_elbow_wrist(const geometry_msgs::Pose pose, std::vector<double> *q_vector, uint32_t constrain);
int RM_inverse_constrain_shoulder_elbow_wrist(const Eigen::Matrix4d T, std::vector<double> *q_vector, uint32_t constrain);
int RM_inverse_constrain_shoulder_elbow_wrist(const Eigen::MatrixXd T, std::vector<double> *q_vector, uint32_t constrain);
int RM_inverse_constrain_shoulder_elbow_wrist(const double* T, double* q_sols, uint32_t constrain);
// --------------------------------------------------------------------------------------------------------------
// 几何雅各比矩阵
void RM_Jacobian(const double* q, double* J);
void RM_JacobianMatrix_From_JointVector(const Eigen::VectorXd joint, Eigen::MatrixXd* Jacobian);
// --------------------------------------------------------------------------------------------------------------
// 寻找最近关节值
int RM_find_nearest_joint_index(std::vector<Eigen::VectorXd> q_vector_group, int slov_num, Eigen::VectorXd* q_vector);
int RM_find_nearest_joint_index(double* q_sols, int slov_num, double* q_ref);

// --------------------------------------------------------------------------------------------------------------
// 逆解中寻优
int RM_find_nearest_joint_index(std::vector<Eigen::VectorXd> q_vector_group, int slov_num, Eigen::VectorXd q_ref, Eigen::VectorXd* q_result);
bool RM_tarMat_nearJ_bestJ(double* trans, std::vector<double> nearJ, std::vector<double> *bestJ);
bool RM_tarPose_nearJ_bestJ(geometry_msgs::Pose tarPose, std::vector<double> nearJ, std::vector<double> *bestJ);
bool RM_tarTrans_nearJ_bestJ(tf::StampedTransform tarTrans, std::vector<double> nearJ, std::vector<double> *bestJ);

bool RM_Inverse_nearJ_bestJ(const Eigen::Matrix4d T, Eigen::VectorXd q_ref, Eigen::VectorXd *q_best);
bool RM_Inverse_nearJ_bestJ(const Eigen::MatrixXd T, Eigen::VectorXd q_ref, Eigen::VectorXd *q_best);
bool RM_Inverse_nearJ_bestJ(const geometry_msgs::Pose pose, Eigen::VectorXd q_ref, Eigen::VectorXd *q_best);
// ==============================================================================================================
// 基本数学工具
int SIGN(double x) {
      return (x > 0) - (x < 0);
    }

// --------------------------------------------------------------------------------------------------------------
// 正解
void RM_forward(const double* q, double* T)
{
    double s1 = sin(*q), c1 = cos(*q); q++;
    double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
    double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++;
    q234 += *q; q++;
    double s5 = sin(*q), c5 = cos(*q); q++;
    double s6 = sin(*q), c6 = cos(*q); 
    double s234 = sin(q234), c234 = cos(q234);
    
    *T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++;
    *T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) - 
          (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++;
    *T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 - 
          s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++;
    *T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 - 
          d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 - 
          a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++;
    *T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++;
    *T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) + 
          s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++;
    *T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) - 
          s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++;
    *T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 + 
          (d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 - 
          a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++;
    *T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++;
    *T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++;
    *T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++;
    *T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 - 
         (d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++;
    *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
}

void RM_forward(Eigen::VectorXd joint, Eigen::Matrix4d* mat)
{
  double* T = new double[16];
  double* q = new double[6];

  for(int i = 0; i < 6; i ++)
  {
    q[i] = joint(i);
  }

  RM_forward(q, T);

  for (int i = 0; i < 4; i ++)
  {
    for (int j = 0; j < 4; j ++)
    {
      (*mat)(i,j) = T[4*i+j];
    }
  }
}
void RM_forward(Eigen::VectorXd joint, geometry_msgs::Pose* pose)
{
  Eigen::Matrix4d mat;
  RM_forward(joint, &mat);
  RM_Matrix_to_Pose1(mat, pose);
}
void RM_forward_all(const double* q, double* T1, double* T2, double* T3, 
                                  double* T4, double* T5, double* T6) 
{
  double s1 = sin(*q), c1 = cos(*q); q++; // q1
  double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; // q2
  double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++; // q3
  q234 += *q; q++; // q4
  double s5 = sin(*q), c5 = cos(*q); q++; // q5
  double s6 = sin(*q), c6 = cos(*q); // q6
  double s23 = sin(q23), c23 = cos(q23);
  double s234 = sin(q234), c234 = cos(q234);

  if(T1 != NULL) {
    *T1 = c1; T1++;    *T1 = 0; T1++;    *T1 = s1;  T1++;    *T1 = 0;  T1++;
    *T1 = s1; T1++;    *T1 = 0; T1++;    *T1 = -c1; T1++;    *T1 = 0;  T1++;
    *T1 = 0;  T1++;    *T1 = 1; T1++;    *T1 = 0;   T1++;    *T1 = d1; T1++;
    *T1 = 0;  T1++;    *T1 = 0; T1++;    *T1 = 0;   T1++;    *T1 = 1;  T1++;
  }

  if(T2 != NULL) {
    *T2 = c1*c2; T2++;    *T2 = -c1*s2; T2++;    *T2 = s1;  T2++;    *T2 = a2*c1*c2;   T2++;
    *T2 = c2*s1; T2++;    *T2 = -s1*s2; T2++;    *T2 = -c1; T2++;    *T2 = a2*c2*s1;   T2++;
    *T2 = s2;    T2++;    *T2 = c2;     T2++;    *T2 = 0;   T2++;    *T2 = d1 + a2*s2; T2++;
    *T2 = 0;     T2++;    *T2 = 0;      T2++;    *T2 = 0;   T2++;    *T2 = 1;          T2++;
  }

  if(T3 != NULL) {
    *T3 = c23*c1; T3++;
    *T3 = -s23*c1; T3++;
    *T3 = s1; T3++;
    *T3 = c1*(a3*c23 + a2*c2); T3++;
    *T3 = c23*s1; T3++;
    *T3 = -s23*s1; T3++;
    *T3 = -c1; T3++;
    *T3 = s1*(a3*c23 + a2*c2); T3++;
    *T3 = s23; T3++;
    *T3 = c23; T3++;
    *T3 = 0; T3++;
    *T3 = d1 + a3*s23 + a2*s2; T3++;
    *T3 = 0; T3++;
    *T3 = 0; T3++;
    *T3 = 0; T3++;
    *T3 = 1; T3++;
  }

  if(T4 != NULL) {
    *T4 = c234*c1; T4++;
    *T4 = s1; T4++;
    *T4 = s234*c1; T4++;
    *T4 = c1*(a3*c23 + a2*c2) + d4*s1; T4++;
    *T4 = c234*s1; T4++;
    *T4 = -c1; T4++;
    *T4 = s234*s1; T4++;
    *T4 = s1*(a3*c23 + a2*c2) - d4*c1; T4++;
    *T4 = s234; T4++;
    *T4 = 0; T4++;
    *T4 = -c234; T4++;
    *T4 = d1 + a3*s23 + a2*s2; T4++;
    *T4 = 0; T4++;
    *T4 = 0; T4++;
    *T4 = 0; T4++;
    *T4 = 1; T4++;
  }

  if(T5 != NULL) {
    *T5 = s1*s5 + c234*c1*c5; T5++;
    *T5 = -s234*c1; T5++;
    *T5 = c5*s1 - c234*c1*s5; T5++;
    *T5 = c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++;
    *T5 = c234*c5*s1 - c1*s5; T5++;
    *T5 = -s234*s1; T5++;
    *T5 = - c1*c5 - c234*s1*s5; T5++;
    *T5 = s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++;
    *T5 = s234*c5; T5++;
    *T5 = c234; T5++;
    *T5 = -s234*s5; T5++;
    *T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++;
    *T5 = 0; T5++;
    *T5 = 0; T5++;
    *T5 = 0; T5++;
    *T5 = 1; T5++;
  }

  if(T6 != NULL) {
    *T6 =   c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++;
    *T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++;
    *T6 = c5*s1 - c234*c1*s5; T6++;
    *T6 = d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++;
    *T6 = - c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T6++;
    *T6 = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T6++;
    *T6 = - c1*c5 - c234*s1*s5; T6++;
    *T6 = s1*(a3*c23 + a2*c2) - d4*c1 - d6*(c1*c5 + c234*s1*s5) + d5*s234*s1; T6++;
    *T6 = c234*s6 + s234*c5*c6; T6++;
    *T6 = c234*c6 - s234*c5*s6; T6++;
    *T6 = -s234*s5; T6++;
    *T6 = d1 + a3*s23 + a2*s2 - d5*c234 - d6*s234*s5; T6++;
    *T6 = 0; T6++;
    *T6 = 0; T6++;
    *T6 = 0; T6++;
    *T6 = 1; T6++;
  }
}

// --------------------------------------------------------------------------------------------------------------
// 逆解
int RM_inverse_limited(const geometry_msgs::Pose pose, std::vector<Eigen::VectorXd> *q_vector)
{
  Eigen::Matrix4d T;
  RM_Pose_to_Matrix1(pose, &T);
  int num = RM_inverse_limited(T, q_vector);

  // std::cout << "RM_inverse_limited func 1 : " << std::endl;
  // std::cout << "num : " << num << std::endl;

  return num;
}
int RM_inverse_limited(const Eigen::Matrix4d T, std::vector<Eigen::VectorXd> *q_vector)
{
  Eigen::MatrixXd Trans(4,4);
  Trans.block<4,4>(0,0) = T.block<4,4>(0,0);
  int num = RM_inverse_limited(Trans, q_vector);

  // std::cout << "RM_inverse_limited func 2 : " << std::endl;
  // std::cout << "num : " << num << std::endl;

  return num;
}
int RM_inverse_limited(const Eigen::MatrixXd T, std::vector<Eigen::VectorXd> *q_vector)
{
  int num = RM_inverse(T, q_vector);

  for (int i = 0; i < num; i ++)
  {
    for(int j = 0; j < 6; j++)
    {
      if ( j == 5)
      {
        if ((*q_vector)[i](j) > MATH_PI*0.5)
        {
          (*q_vector)[i](j) -= 2.0*MATH_PI;
        }
      }
      else
      {
        if ((*q_vector)[i](j) > MATH_PI)
        {
          (*q_vector)[i](j) -= 2.0*MATH_PI;
        }
        else if((*q_vector)[i](j) < -MATH_PI)
        {
          (*q_vector)[i](j) += 2.0*MATH_PI;
        }
      }
      
    }
  }

  // std::cout << "RM_inverse_limited func 3 : " << std::endl;
  // std::cout << "num : " << num << std::endl;

  return num;
}

int RM_inverse(const Eigen::MatrixXd T, std::vector<Eigen::VectorXd> *q_vector)
{
  double aT[16];
  for (int i = 0; i < 4; i ++)
  {
    for (int j = 0; j < 4; j ++)
    {
      aT[i*4 + j] = T(i,j);
    }    
  }
  double q_sols[8*6];
  int num = RM_inverse(aT, q_sols, 0.0);
  // q_vector->resize(num);
  if (num != 0)
  {
    for (int i = 0; i < num; i ++)
    {
      // std::cout << "RM_inverse -  for" << i << std::endl;
      (*q_vector)[i] << q_sols[6*i], q_sols[6*i+1], q_sols[6*i+2],
                    q_sols[6*i+3], q_sols[6*i+4], q_sols[6*i+5];
    }
  }

  // std::cout << "RM_inverse_limited func 4 : " << std::endl;
  // std::cout << "num : " << num << std::endl;

  return num;
}

// 角度计算过程：
// q1[2] -> q5[2][2] -> q6 -> q3[2] -> q2[2] -> q4[2]
int RM_inverse(const double* T, double* q_sols, double q6_des) {
    int num_sols = 0;
    double T02 = -*T; T++; double T00 =  *T; T++; double T01 =  *T; T++; double T03 = -*T; T++; 
    double T12 = -*T; T++; double T10 =  *T; T++; double T11 =  *T; T++; double T13 = -*T; T++; 
    double T22 =  *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 =  *T;

    // shoulder rotate joint (q1) //
    double q1[2];
    {
      double A = d6*T12 - T13;
      double B = d6*T02 - T03;
      double R = A*A + B*B;
      if(fabs(A) < ZERO_THRESH) {
        double div;
        if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
          div = -SIGN(d4)*SIGN(B);
        else
          div = -d4/B;
        double arcsin = asin(div);
        if(fabs(arcsin) < ZERO_THRESH)
          arcsin = 0.0;
        if(arcsin < 0.0)
          q1[0] = arcsin + 2.0*PI;
        else
          q1[0] = arcsin;
        q1[1] = PI - arcsin;
      }
      else if(fabs(B) < ZERO_THRESH) {
        double div;
        if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
          div = SIGN(d4)*SIGN(A);
          
        else
          div = d4/A;
        double arccos = acos(div);
        q1[0] = arccos;
        q1[1] = 2.0*PI - arccos;
      }
      else if(d4*d4 > R) {
        return num_sols;
      }
      else {
        double arccos = acos(d4 / sqrt(R)) ;
        double arctan = atan2(-B, A);
        double pos = arccos + arctan;
        double neg = -arccos + arctan;
        if(fabs(pos) < ZERO_THRESH) pos = 0.0;
        if(fabs(neg) < ZERO_THRESH) neg = 0.0;
        // shoulder left
        if(pos >= 0.0)  q1[0] = pos;
        else  q1[0] = 2.0*PI + pos;
        // shoulder right
        if(neg >= 0.0)  q1[1] = neg; 
        else  q1[1] = 2.0*PI + neg;
      }
    }
    

    // wrist 2 joint (q5) //
    double q5[2][2];
    {
      for(int i=0;i<2;i++) {
        double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);
        double div;
        // gaokaikai: acos计算溢出
        // if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
        if((fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH) || (fabs(numer) > (fabs(d6) + ZERO_THRESH)))
          div = SIGN(numer) * SIGN(d6);
        else
          div = numer / d6;
        double arccos = acos(div);

        // wrist-in
        q5[i][0] = arccos;
        // wrist-out
        q5[i][1] = 2.0*PI - arccos;

        // std::cout << "numer = " << numer << ", "
        //           << "div = " << div  << ", "
        //           << "arccos = " << arccos  << ", "
        //           << std::endl;
      }

      // std::cout << "q5 : " << q5[0][0] << ", " << q5[0][1] << ", " << q5[1][0] << ", " << q5[1][1] << ", " << std::endl;
    }
    

    {
      for(int i=0;i<2;i++) {
        for(int j=0;j<2;j++) {
          double c1 = cos(q1[i]), s1 = sin(q1[i]);
          double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
          double q6;
          // wrist 3 joint (q6) //
          if(fabs(s5) < ZERO_THRESH)
            q6 = q6_des;
          else {
            q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1), 
                       SIGN(s5)*(T00*s1 - T10*c1));
            if(fabs(q6) < ZERO_THRESH)
              q6 = 0.0;
            if(q6 < 0.0)
              q6 += 2.0*PI;
          }
          

          double q2[2], q3[2], q4[2];
          / RRR joints (q2,q3,q4) 
          double c6 = cos(q6), s6 = sin(q6);
          double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
          double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
          double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) + 
                        T03*c1 + T13*s1;
          double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);

          double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
          // std::cout << "q1 = " << q1[i];
          // std::cout << "  q5 = " << q5[i][j];
          // std::cout << "  c3 = " << c3 << std::endl;
          if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
            c3 = SIGN(c3);
          else if(fabs(c3) > 1.0) {
            // TODO NO SOLUTION
            continue;
          }
          double arccos = acos(c3);

          // elbow-in
          q3[0] = arccos;
          // elbow-out
          q3[1] = 2.0*PI - arccos;

          double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
          double s3 = sin(arccos);
          double A = (a2 + a3*c3), B = a3*s3;
          q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
          q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);

          double c23_0 = cos(q2[0]+q3[0]);
          double s23_0 = sin(q2[0]+q3[0]);
          double c23_1 = cos(q2[1]+q3[1]);
          double s23_1 = sin(q2[1]+q3[1]);
          q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
          q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);

          // std::cout << "q4 : " << q4[0] << ", " << q4[1] << std::endl;

          
          for(int k=0;k<2;k++) {
            if(fabs(q2[k]) < ZERO_THRESH)
              q2[k] = 0.0;
            else if(q2[k] < 0.0) q2[k] += 2.0*PI;
            if(fabs(q4[k]) < ZERO_THRESH)
              q4[k] = 0.0;
            else if(q4[k] < 0.0) q4[k] += 2.0*PI;
            q_sols[num_sols*6+0] = q1[i];    q_sols[num_sols*6+1] = q2[k]; 
            q_sols[num_sols*6+2] = q3[k];    q_sols[num_sols*6+3] = q4[k]; 
            q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6; 
            num_sols++;
          }

        }
      }
    }

    return num_sols;
}

int RM_inverse_constrain_shoulder_elbow_wrist(const geometry_msgs::Pose pose, std::vector<double> *q_vector, uint32_t constrain)
{
  Eigen::Matrix4d Trans;
  RM_Pose_to_Matrix1(pose, &Trans);
  int num = RM_inverse_constrain_shoulder_elbow_wrist(Trans, q_vector, constrain);
  // std::cout << "RM_inverse_constrain_shoulder_elbow_wrist 1 , result" << num << std::endl;
  // std::cout << "Trans : " << Trans << std::endl;
  return num;
}

int RM_inverse_constrain_shoulder_elbow_wrist(const Eigen::Matrix4d T, std::vector<double> *q_vector, uint32_t constrain)
{
  Eigen::MatrixXd Trans(4,4);
  Trans.block<4,4>(0,0) = T.block<4,4>(0,0);
  int num = RM_inverse_constrain_shoulder_elbow_wrist(Trans, q_vector, constrain);
  // std::cout << "RM_inverse_constrain_shoulder_elbow_wrist 2 , result" << num << std::endl;
  return num;
}

int RM_inverse_constrain_shoulder_elbow_wrist(const Eigen::MatrixXd T, std::vector<double> *q_vector, uint32_t constrain)
{
  double aT[16];
  for (int i = 0; i < 4; i ++)
  {
    for (int j = 0; j < 4; j ++)
    {
      aT[i*4 + j] = T(i,j);
    }    
  }
  double q_sols[6];
  int num = RM_inverse_constrain_shoulder_elbow_wrist(aT, q_sols, constrain);
  // q_vector->resize(num);
  if (num != 0)
  {
    for (int i = 0; i < 6; i ++)
    {
      (*q_vector)[i] = q_sols[i];
    }
  }
  // std::cout << "RM_inverse_constrain_shoulder_elbow_wrist 3 , result" << num << std::endl;
  return num;
}


int RM_inverse_constrain_shoulder_elbow_wrist(const double* T, double* q_sols, uint32_t constrain) {
    double q6_des = 0.0;
    uint32_t constrain_wrist = (constrain&0x00000001);
    uint32_t constrain_elbow = (constrain&0x00000002)>>1;
    uint32_t constrain_shoulder = (constrain&0x00000004)>>2;

    int num_sols = 0;
    double T02 = -*T; T++; double T00 =  *T; T++; double T01 =  *T; T++; double T03 = -*T; T++; 
    double T12 = -*T; T++; double T10 =  *T; T++; double T11 =  *T; T++; double T13 = -*T; T++; 
    double T22 =  *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 =  *T;

    // shoulder rotate joint (q1) //
    double q1_constrain = 0.0;
    double q1[2];
    {
      double A = d6*T12 - T13;
      double B = d6*T02 - T03;
      double R = A*A + B*B;
      if(fabs(A) < ZERO_THRESH) {
        double div;
        if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
          div = -SIGN(d4)*SIGN(B);
        else
          div = -d4/B;
        double arcsin = asin(div);
        if(fabs(arcsin) < ZERO_THRESH)
          arcsin = 0.0;
        if(arcsin < 0.0)
          q1[0] = arcsin + 2.0*PI;
        else
          q1[0] = arcsin;
        q1[1] = PI - arcsin;
      }
      else if(fabs(B) < ZERO_THRESH) {
        double div;
        if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
          div = SIGN(d4)*SIGN(A);
          
        else
          div = d4/A;
        double arccos = acos(div);
        q1[0] = arccos;
        q1[1] = 2.0*PI - arccos;
      }
      else if(d4*d4 > R) {
        std::cout << "return because : d4*d4 > R" << d4 << " , " << R << std::endl;
        return num_sols;
      }
      else {
        double arccos = acos(d4 / sqrt(R)) ;
        double arctan = atan2(-B, A);
        double pos = arccos + arctan;
        double neg = -arccos + arctan;
        if(fabs(pos) < ZERO_THRESH) pos = 0.0;
        if(fabs(neg) < ZERO_THRESH) neg = 0.0;
        // shoulder left
        if(pos >= 0.0)  q1[0] = pos;
        else  q1[0] = 2.0*PI + pos;
        // shoulder right
        if(neg >= 0.0)  q1[1] = neg; 
        else  q1[1] = 2.0*PI + neg;
      }
    }

    if (constrain_shoulder == 1)
    {
      q1_constrain  = q1[0];
    }
    else
    {
       q1_constrain  = q1[1];
    }

    
    

    // wrist 2 joint (q5) //
    double q5_constrain = 0.0;
    double q5[2];
    {
        double numer = (T03*sin(q1_constrain) - T13*cos(q1_constrain)-d4);
        double div;
        // gaokaikai: acos计算溢出
        // if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
        if((fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH) || (fabs(numer) > (fabs(d6) + ZERO_THRESH)))
          div = SIGN(numer) * SIGN(d6);
        else
          div = numer / d6;
        double arccos = acos(div);

        // wrist-in
        q5[0] = arccos;
        // wrist-out
        q5[1] = 2.0*PI - arccos;
    }

    if (constrain_wrist == 1)
    {
      q5_constrain  = q5[0];
    }
    else
    {
      q5_constrain  = q5[1];
    }
    
    

    {
      double c1 = cos(q1_constrain), s1 = sin(q1_constrain);
      double c5 = cos(q5_constrain), s5 = sin(q5_constrain);
      double q6;
      // wrist 3 joint (q6) //
      if(fabs(s5) < ZERO_THRESH)
        q6 = q6_des;
      else {
        q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1), 
                    SIGN(s5)*(T00*s1 - T10*c1));
        if(fabs(q6) < ZERO_THRESH)
          q6 = 0.0;
        if(q6 < 0.0)
          q6 += 2.0*PI;
      }
      
      double q2_constrain = 0.0;
      double q3_constrain = 0.0;
      double q4_constrain = 0.0;
      double q2[2], q3[2], q4[2];
      / RRR joints (q2,q3,q4) 
      double c6 = cos(q6), s6 = sin(q6);
      double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
      double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
      double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) + 
                    T03*c1 + T13*s1;
      double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);

      double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
      if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
        c3 = SIGN(c3);
      else if(fabs(c3) > 1.0) {
        // TODO NO SOLUTION
        std::cout << "q1 = " << q1_constrain << "   q5 = " << q5_constrain << std::endl;
        std::cout << "return because : fabs(c3) > 1.0 ; " << c3 << std::endl;
        return 0;
      }
      double arccos = acos(c3);

      // elbow-in
      q3[0] = arccos;
      // elbow-out
      q3[1] = 2.0*PI - arccos;

      if (constrain_elbow == 1)
      {
        q3_constrain  = q3[0];
      }
      else
      {
        q3_constrain  = q3[1];
      }



      double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
      double s3 = sin(arccos);
      double A = (a2 + a3*c3), B = a3*s3;
      q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
      q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);

      if (constrain_elbow == 1)
      {
        q2_constrain  = q2[0];
      }
      else
      {
        q2_constrain  = q2[1];
      }

      double c23_0 = cos(q2[0]+q3[0]);
      double s23_0 = sin(q2[0]+q3[0]);
      double c23_1 = cos(q2[1]+q3[1]);
      double s23_1 = sin(q2[1]+q3[1]);
      q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
      q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);

      if (constrain_elbow == 1)
      {
        q4_constrain  = q4[0];
      }
      else
      {
        q4_constrain  = q4[1];
      }

      
      {
        if(fabs(q2_constrain) < ZERO_THRESH)
          q2_constrain = 0.0;
        else if(q2_constrain < 0.0) q2_constrain += 2.0*PI;
        if(fabs(q4_constrain) < ZERO_THRESH)
          q4_constrain = 0.0;
        else if(q4_constrain < 0.0) q4_constrain += 2.0*PI;
        q_sols[0] = q1_constrain;  q_sols[1] = q2_constrain; 
        q_sols[2] = q3_constrain; q_sols[3] = q4_constrain;
        q_sols[4] = q5_constrain;  q_sols[5] = q6; 
        num_sols++;
      }
    }

    for (int i = 0; i < 6; i ++)
    {
      if (q_sols[i] > MATH_PI)
      {
        q_sols[i] -= 2.0*MATH_PI;
      }
    }

    // std::cout << "num_sols : " << num_sols << std::endl;
    // std::cout << "q_sols : " << q_sols[0] << " , " 
    //           << q_sols[1] << " , " 
    //           << q_sols[2] << " , " 
    //           << q_sols[3] << " , " 
    //           << q_sols[4] << " , " 
    //           << q_sols[5] << " , " 
    //           << std::endl;

    return num_sols;
}


// uint32_t RM_joint_config(std::vector<double> *q_vector)
// {
//   uint32_t joint_config = 0x0000;

//   uint32_t wirst = 0;
//   if (q_vector[4] > 0.0)
//   {
//     wirst = 0x0001;
//   }

//   uint32_t elbow = 0;
//   if (q_vector[2] > 0.0)
//   {
//     elbow = 0x0001;
//   }

//   uint32_t shoulder = 0;

//   double q[6] = {q_vector[0], q_vector[1], q_vector[2], q_vector[3], q_vector[4], q_vector[5]};
//   double T1[16];
//   double T2[16];
//   double T3[16];
//   double T4[16];
//   double T5[16];
//   double T6[16];
//   RM_forward_all(cq, double* T1, double* T2, double* T3, 
//                                   double* T4, double* T5, double* T6);
  
//   return joint_config;
// }

int RM_inverse_jointlimited(const double* T, double* q_sols, double q6_des)
{
  int num_sols = RM_inverse(T, q_sols, q6_des);
  for(int i=0; i<num_sols; i++)
  {
      for (int j=0; j<6; j++)
      {
        if (q_sols[i*6+j] > Jlimit_max[j])
        {
            q_sols[i*6+j] -= 2*PI;
        }
        else if (q_sols[i*6+j] < Jlimit_min[j])
        {
            q_sols[i*6+j] += 2*PI;
        }
        else
        {
            // none
        }
      }
  }
  return num_sols;
}


// --------------------------------------------------------------------------------------------------------------
// 几何雅各比矩阵

void RM_Jacobian(const double* q, double* J)
{
    double* T1 = new double[16];
    double* T2 = new double[16];
    double* T3 = new double[16];
    double* T4 = new double[16];
    double* T5 = new double[16];
    double* T6 = new double[16];

    RM_forward_all(q, T1, T2, T3, T4, T5, T6);

    double pe[3] = {T6[3], T6[3+4], T6[3+8]};
    double jpi[3] = {0.0, 0.0, 0.0};

    // J1
    double z0[3] = {0.0, 0.0, 1.0};
    double p0[3] = {0.0, 0.0, 0.0};
    double pe_minus_p0[3] = {pe[0]-p0[0], pe[1]-p0[1], pe[2]-p0[2]};

    RM_vector3_cross_mult(z0, pe_minus_p0, jpi);
    double J1[6] = {jpi[0], jpi[1], jpi[2], z0[0], z0[1], z0[2]};

    // printf("pe_minus_p0 = [%1.4f, %1.4f, %1.4f] \n", pe_minus_p0[0], pe_minus_p0[1], pe_minus_p0[2]);

    // J2
    double z1[3] = {T1[2], T1[2+4], T1[2+8]};
    double p1[3] = {T1[3], T1[3+4], T1[3+8]};
    double pe_minus_p1[3] = {pe[0]-p1[0], pe[1]-p1[1], pe[2]-p1[2]};

    RM_vector3_cross_mult(z1, pe_minus_p1, jpi);
    double J2[6] = {jpi[0], jpi[1], jpi[2], z1[0], z1[1], z1[2]};

    // J3
    double z2[3] = {T2[2], T2[2+4], T2[2+8]};
    double p2[3] = {T2[3], T2[3+4], T2[3+8]};
    double pe_minus_p2[3] = {pe[0]-p2[0], pe[1]-p2[1], pe[2]-p2[2]};

    RM_vector3_cross_mult(z2, pe_minus_p2, jpi);
    double J3[6] = {jpi[0], jpi[1], jpi[2], z2[0], z2[1], z2[2]};

    // J4
    double z3[3] = {T3[2], T3[2+4], T3[2+8]};
    double p3[3] = {T3[3], T3[3+4], T3[3+8]};
    double pe_minus_p3[3] = {pe[0]-p3[0], pe[1]-p3[1], pe[2]-p3[2]};

    RM_vector3_cross_mult(z3, pe_minus_p3, jpi);
    double J4[6] = {jpi[0], jpi[1], jpi[2], z3[0], z3[1], z3[2]};

    // J5
    double z4[3] = {T4[2], T4[2+4], T4[2+8]};
    double p4[3] = {T4[3], T4[3+4], T4[3+8]};
    double pe_minus_p4[3] = {pe[0]-p4[0], pe[1]-p4[1], pe[2]-p4[2]};

    RM_vector3_cross_mult(z4, pe_minus_p4, jpi);
    double J5[6] = {jpi[0], jpi[1], jpi[2], z4[0], z4[1], z4[2]};

    // J6
    double z5[3] = {T5[2], T5[2+4], T5[2+8]};
    double p5[3] = {T5[3], T5[3+4], T5[3+8]};
    double pe_minus_p5[3] = {pe[0]-p5[0], pe[1]-p5[1], pe[2]-p5[2]};
    
    RM_vector3_cross_mult(z5, pe_minus_p5, jpi);
    double J6[6] = {jpi[0], jpi[1], jpi[2], z5[0], z5[1], z5[2]};

    // printf("T1 x y z = [%1.4f, %1.4f, %1.4f] \n", p1[0], p1[1], p1[2]);
    // printf("T2 x y z = [%1.4f, %1.4f, %1.4f] \n", p2[0], p2[1], p2[2]);
    // printf("T3 x y z = [%1.4f, %1.4f, %1.4f] \n", p3[0], p3[1], p3[2]);
    // printf("T4 x y z = [%1.4f, %1.4f, %1.4f] \n", p4[0], p4[1], p4[2]);
    // printf("T5 x y z = [%1.4f, %1.4f, %1.4f] \n", p5[0], p5[1], p5[2]);
    // printf("T6 x y z = [%1.4f, %1.4f, %1.4f] \n", T6[3], T6[3+4], T6[3+8]);

    for (int i = 0; i < 6; i ++)
    {
      J[i*6 + 0] = J1[i];
      J[i*6 + 1] = J2[i];
      J[i*6 + 2] = J3[i];
      J[i*6 + 3] = J4[i];
      J[i*6 + 4] = J5[i];
      J[i*6 + 5] = J6[i];
    }

}

void RM_JacobianMatrix_From_JointVector(const Eigen::VectorXd joint, Eigen::MatrixXd* Jacobian)
{
    double* q = new double[6];
    double* J = new double[36];

    for (int i = 0; i < 6; i ++)
    {
      q[i] = joint(i);
    }
    RM_Jacobian(q, J);
    for (int i = 0; i < 6; i ++)
    {
      for (int j = 0; j < 6; j ++)
      {
        (*Jacobian)(i,j) = J[i*6+j];
      }
    }
}
// --------------------------------------------------------------------------------------------------------------
// 寻找最近关节值
int RM_find_nearest_joint_index(std::vector<Eigen::VectorXd> q_vector_group, int slov_num, Eigen::VectorXd q_ref, Eigen::VectorXd* q_result)
{
    double q_sols[6*slov_num];
    double q_ref_array[6];
    int rst_index = 0;
    for (int i = 0; i < slov_num; i ++)
    {
      for (int j = 0; j < 6; j ++)
      {
        q_sols[6*i+j] = q_vector_group[i](j);
      }
    }

    for (int j = 0; j < 6; j ++)
    {
      q_ref_array[j] = q_ref(j);
    }

    rst_index = RM_find_nearest_joint_index(q_sols, slov_num, q_ref_array);

    *q_result = q_vector_group[rst_index];

    return rst_index;
}

int RM_find_nearest_joint_index(double* q_sols, int slov_num, double* q_ref)
{
    double error_this_group = 0.0;
    double err_min = 0.0;
    int min_index = 0;
    for (int index = 0; index < slov_num; index++)
    {
      error_this_group = 0.0;
      for (int q = 0; q < 6; q ++)
      {
        double err_q = fabs(q_sols[index*6 + q] - q_ref[q]);
        if (err_q > MATH_PI)
        {
          err_q = 2.0*MATH_PI - err_q;
        }

        error_this_group += err_q;
      }
      
       if (index == 0)
       {
           err_min = error_this_group;
           min_index = 0;
       }
       else
       {
           if (err_min > error_this_group)
           {
               err_min = error_this_group;
               min_index = index;
           }
       }
    }
    return min_index;
}
// int RM_find_nearest_joint_index(double* q_sols, int slov_num, double* q_ref)
// {
//     int index = 0;
//     double error[slov_num];
//     double err_min = 0.0;
//     int min_index = 0;
//     for (index = 0; index < slov_num; index++)
//     {
//        error[index] = std::fabs(q_sols[index*6 + 0] - q_ref[0])
//                     + std::fabs(q_sols[index*6 + 1] - q_ref[1])
//                     + std::fabs(q_sols[index*6 + 2] - q_ref[2])
//                     + std::fabs(q_sols[index*6 + 3] - q_ref[3])
//                     + std::fabs(q_sols[index*6 + 4] - q_ref[4])
//                     + std::fabs(q_sols[index*6 + 5] - q_ref[5]);
//        if (index == 0)
//        {
//            err_min = error[index];
//            min_index = 0;
//        }
//        else
//        {
//            if (err_min > error[index])
//            {
//                err_min = error[index];
//                min_index = index;
//            }
//        }
//        // printf("error[%d] = %1.4f \n", index, error[index]);
//     }
//     return min_index;
// }

// --------------------------------------------------------------------------------------------------------------
// 逆解中寻优

bool RM_tarMat_nearJ_bestJ(double* trans, std::vector<double> nearJ, std::vector<double> *bestJ)
{
  double q_sols[8*6];
  double q_near[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  int num_sols;
  int near_index;
     
  num_sols = RM_inverse(trans, q_sols, 0.0);
  if (num_sols == 0)
  {
      return false;
  }

  for(int i=0; i<num_sols; i++)
  {
      for (int j=0; j<6; j++)
      {
          if (q_sols[i*6+j] > Jlimit_max[j])
          {
              q_sols[i*6+j] -= 2*PI;
          }
          else if (q_sols[i*6+j] < Jlimit_min[j])
          {
              q_sols[i*6+j] += 2*PI;
          }
          else
          {
              // none
          }
      }      
      // printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n", 
      // q_sols[i*6+0], q_sols[i*6+1], q_sols[i*6+2], q_sols[i*6+3], q_sols[i*6+4], q_sols[i*6+5]);
  }

  for (int i=0; i <6; i ++)
  {
      q_near[i] = (double)(nearJ[i]);
  }
  near_index = RM_find_nearest_joint_index(q_sols, num_sols, q_near);
  // printf("near_index is :%d \n", near_index);

  for (int i = 0; i <6; i ++)
  {
      (*bestJ)[i] = (double)(q_sols[near_index*6+i]);
      // printf("bestJ[%d] = %1.4f \n", i, (*bestJ)[i]);
  }

  return true;
}

bool RM_tarPose_nearJ_bestJ(geometry_msgs::Pose tarPose, std::vector<double> nearJ, std::vector<double> *bestJ)
{
  double* trans = new double[16];
  double q_sols[8*6];
  double q_near[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  int num_sols;
  int near_index;

  RM_Pose_to_Matrix(tarPose, trans);

    // // TODO:test
    // Eigen::MatrixXd fbkTrans(4,4);
    // Robot_Pose_to_Matrix1(tarPose, &fbkTrans);

  return RM_tarMat_nearJ_bestJ(trans, nearJ, bestJ);
}

bool RM_tarTrans_nearJ_bestJ(tf::StampedTransform tarTrans, std::vector<double> nearJ, std::vector<double> *bestJ)
{
  geometry_msgs::Pose tarPose;
  tarPose.position.x = tarTrans.getOrigin().x();
  tarPose.position.y = tarTrans.getOrigin().y();
  tarPose.position.z = tarTrans.getOrigin().z();
  tarPose.orientation.x = tarTrans.getRotation().x();
  tarPose.orientation.y = tarTrans.getRotation().y();
  tarPose.orientation.z = tarTrans.getRotation().z();
  tarPose.orientation.w = tarTrans.getRotation().w();
  return RM_tarPose_nearJ_bestJ(tarPose, nearJ, bestJ);
}

bool RM_Inverse_nearJ_bestJ(const Eigen::Matrix4d T, Eigen::VectorXd q_ref, Eigen::VectorXd *q_best)
{
  geometry_msgs::Pose pose;
  RM_Matrix_to_Pose1(T, &pose);
  return RM_Inverse_nearJ_bestJ(pose, q_ref, q_best);
}

bool RM_Inverse_nearJ_bestJ(const Eigen::MatrixXd T, Eigen::VectorXd q_ref, Eigen::VectorXd *q_best)
{
  Eigen::MatrixXd Trans(4,4);
  Trans.block<4,4>(0,0) = T.block<4,4>(0,0);
  return RM_Inverse_nearJ_bestJ(Trans, q_ref, q_best);
}

bool RM_Inverse_nearJ_bestJ(const geometry_msgs::Pose pose, Eigen::VectorXd q_ref, Eigen::VectorXd *q_best)
{
  std::vector<double> nearJ;
  nearJ.resize(6);
  for (int i = 0; i < 6; i ++)
  {
    nearJ[i] = q_ref(i);
  }

  std::vector<double> bestJ;
  bestJ.resize(6);
  

  bool result = RM_tarPose_nearJ_bestJ(pose, nearJ, &bestJ);
  if (result)
  {
    for (int i = 0; i < 6; i ++)
    {
      (*q_best)(i) = bestJ[i];
    }
  }

  return result;
}

/* no more */