moveit获取末端相对于基座的齐次矩阵的C++程序

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>

int main(int argc, char **argv)
{
ros::init(argc, argv, "moveit_get_end_effector_transform");
ros::NodeHandle nh;

// 设置机械臂规划组
moveit::planning_interface::MoveGroupInterface arm("manipulator");

// 获取当前关节角度
std::vector<double> joint_values = arm.getCurrentJointValues();

// 获取末端执行器的位姿
Eigen::Affine3d end_effector_transform = arm.getEndEffectorTransform();

// 获取基座的位姿
Eigen::Affine3d base_transform = arm.getRobotBaseTransform();

// 计算末端执行器相对于基座的位姿
Eigen::Affine3d end_effector_wrt_base = base_transform.inverse() * end_effector_transform;

// 获取末端执行器相对于基座的齐次矩阵的C
Eigen::Matrix3d C = end_effector_wrt_base.rotation();

ROS_INFO_STREAM("End effector transform:\n" << end_effector_wrt_base.matrix());
ROS_INFO_STREAM("C:\n" << C);

return 0;
}