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

以下是使用MoveIt获取末端相对于基座的姿态矩阵的C程序示例：

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <tf2/LinearMath/Quaternion.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "get_end_effector_pose");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit::planning_interface::MoveGroupInterface move_group("manipulator");

  // 获取末端相对于基座的位姿
  geometry_msgs::PoseStamped current_pose = move_group.getCurrentPose("end_effector_link");

  // 将位姿转换为姿态矩阵
  tf2::Quaternion q(current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w);
  tf2::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);

  // 输出姿态矩阵
  ROS_INFO("End Effector Pose Relative to Base:");
  ROS_INFO("X: %f, Y: %f, Z: %f", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
  ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f", roll, pitch, yaw);

  ros::shutdown();
  return 0;
}

在此示例中，我们使用MoveIt的MoveGroupInterface类获取当前末端相对于基座的位姿，并将其转换为姿态矩阵。然后，我们使用tf2库中的Matrix3x3类获取姿态矩阵的滚动、俯仰和偏航角，并将其输出到控制台。