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;
}