ros获取当前机械臂雅克比矩阵的C++程序

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

int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "jacobian_test_node");
ros::NodeHandle nh;

// 创建MoveGroup接口
moveit::planning_interface::MoveGroupInterface move_group("manipulator");

// 获取当前机械臂的关节值
std::vector<double> joint_values;
move_group.getCurrentState()->copyJointGroupPositions(move_group.getCurrentState()->getRobotModel()->getJointModelGroup(move_group.getName()), joint_values);

// 获取机械臂的雅克比矩阵
Eigen::MatrixXd jacobian = move_group.getCurrentState()->getJacobian(move_group.getCurrentState()->getRobotModel()->getJointModelGroup(move_group.getName()));

// 输出雅克比矩阵
ROS_INFO_STREAM("Jacobian matrix: \n" << jacobian);

return 0;
}