moveit求取末端相对于基座的姿态矩阵的C++程序
时间: 2024-05-22 11:11:59 浏览: 19
#include <moveit/move_group_interface/move_group_interface.h>
#include <Eigen/Core>
int main(int argc, char** argv)
{
ros::init(argc, argv, "get_pose_matrix");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// Set up MoveIt!
moveit::planning_interface::MoveGroupInterface move_group("manipulator");
// Get current end effector pose
geometry_msgs::PoseStamped current_pose = move_group.getCurrentPose();
// Convert to Eigen matrix
Eigen::Affine3d current_affine;
tf::poseMsgToEigen(current_pose.pose, current_affine);
// Get rotation matrix
Eigen::Matrix3d rotation_matrix = current_affine.rotation();
// Convert to C array
double rotation_array[9];
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(rotation_array, rotation_matrix.rows(), rotation_matrix.cols()) = rotation_matrix;
// Print matrix
std::cout << "Rotation matrix:\n";
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
std::cout << rotation_array[i * 3 + j] << " ";
}
std::cout << std::endl;
}
return 0;
}
相关推荐
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)