之找到了python版本的获取机械臂雅可比矩阵,没找到c++版本的,于是寻找了一下官方文档:
Robot Model and Robot State — moveit_tutorials Noetic documentation
对应的官方仓库:
把需要改的模型名字改一改,比如机器人的名称和规划组的名称,改称自己模型的名称:
#include <ros/ros.h>
// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/kinematics_metrics/kinematics_metrics.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "robot_model_and_robot_state_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
// BEGIN_TUTORIAL
// Start
// ^^^^^
// Setting up to start using the RobotModel class is very easy. In
// general, you will find that most higher-level components will
// return a shared pointer to the RobotModel. You should always use
// that when possible. In this example, we will start with such a
// shared pointer and discuss only the basic API. You can have a
// look at the actual code API for these classes to get more
// information about how to use more features provided by these
// classes.
//
// We will start by instantiating a
// `RobotModelLoader`_
// object, which will look up
// the robot description on the ROS parameter server and construct a
// :moveit_core:`RobotModel` for us to use.
//
// .. _RobotModelLoader:
// http://docs.ros.org/noetic/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
// Using the :moveit_core:`RobotModel`, we can construct a
// :moveit_core:`RobotState` that maintains the configuration
// of the robot. We will set all joints in the state to their
// default values. We can then get a
// :moveit_core:`JointModelGroup`, which represents the robot
// model for a particular group, e.g. the "panda_arm" of the Panda
// robot.
moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("robot");
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
// Get Joint Values
// ^^^^^^^^^^^^^^^^
// We can retrieve the current set of joint values stored in the state for the Panda arm.
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
// Get the Jacobian
// ^^^^^^^^^^^^^^^^
// We can also get the Jacobian from the :moveit_core:`RobotState`.
// robot_state::RobotState &state(moveit::core::RobotState(kinematic_model));
Eigen::Vector3d reference_point_position(1.0, 0.5, 0.0);
Eigen::MatrixXd jacobian;
kinematic_state->getJacobian(joint_model_group,
kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
reference_point_position, jacobian);
ROS_INFO_STREAM("Jacobian: \n" << jacobian << "\n");
// END_TUTORIAL
ros::shutdown();
return 0;
}
然后运行结果:
但是目前不知道 reference_point_position 具体怎么用,之后知道了再补充。