版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/wxflamy/article/details/79207086
在MoveIt中使用插件基础结构加载运动规划器。这使得MoveIt! 可以在运行的时候加载运动规划器。
开始1
首先，安装moveit!
sudo apt-get install ros-kinetic-moveit等等一系列……的安装，我列不出来，运行的时候缺啥补啥。
先学一下机器人运行状态的计算
roslaunch moveit_tutorials robot_model_and_robot_state_tutorial.launch会得到：
[ INFO] [1532048477.355360139]: Loading robot model 'panda'...
[ INFO] [1532048477.437661595]: Loading robot model 'panda'...
[ INFO] [1532048477.449395230]: Model frame: /panda_link0
[ INFO] [1532048477.449444759]: Joint panda_joint1: 0.000000
[ INFO] [1532048477.449462567]: Joint panda_joint2: 0.000000
[ INFO] [1532048477.449478263]: Joint panda_joint3: 0.000000
[ INFO] [1532048477.449492348]: Joint panda_joint4: 0.000000
[ INFO] [1532048477.449507866]: Joint panda_joint5: 0.000000
[ INFO] [1532048477.449523809]: Joint panda_joint6: 0.000000
[ INFO] [1532048477.449537044]: Joint panda_joint7: 0.000000
[ INFO] [1532048477.449562418]: Current state is not valid
[ INFO] [1532048477.449580281]: Current state is valid
[ INFO] [1532048477.449645241]: Translation:
0.302945
-0.56111
0.671424
[ INFO] [1532048477.449754034]: Rotation:
0.289758 0.409117 0.865254
-0.250031 0.905 -0.34418
-0.923864 -0.116611 0.364522
[ INFO] [1532048477.449933370]: Joint panda_joint1: -1.288419
[ INFO] [1532048477.449947368]: Joint panda_joint2: 1.097801
[ INFO] [1532048477.449962826]: Joint panda_joint3: -2.727456
[ INFO] [1532048477.449977957]: Joint panda_joint4: -0.022056
[ INFO] [1532048477.449991763]: Joint panda_joint5: 1.224717
[ INFO] [1532048477.450005871]: Joint panda_joint6: -2.304395
[ INFO] [1532048477.450019689]: Joint panda_joint7: -1.358359
[ INFO] [1532048477.450453641]: Jacobian:
0.56111 0.0942984 -0.0337196 0.167602 -0.0272619 -0.0268139 3.81639e-17
0.302945 -0.325021 0.0540632 -0.0100692 0.0602333 0.119305 5.55112e-17
0 -0.6233 0.119822 -0.375342 0.121583 -0.0651175 -3.46945e-17
0 0.960396 0.248047 0.828129 0.236901 -0.196984 0.865254
0 0.27864 -0.854951 0.43114 -0.848383 0.435223 -0.34418
1 4.89669e-12 0.455555 0.358219 0.473418 0.878509 0.364522使用kdl_kinematics_plugin/KDLKinematicsPlugin进行的运动学逆运算。
下面看一下具体内容：
//加载机器人模型“robot_description”，并打印参考坐标系
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
//定义一个机器人关节模型组，并获得关节名字。其中“panda_arm”在机器人模型文件中定义
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
//打印当前关节值
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
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]);
}
//将第一个关节设置为一个超过限制的值时，会报错。强制设置后就不报错了。
joint_values[0] = 5.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
/* Check whether any joint is outside its joint limits */
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
/* Enforce the joint limits for this state and check again*/
kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
//下面开始运动学计算，我们把末端设为link8，计算他的末端其次变换并将平移和旋转打印出来。
kinematic_state->setToRandomPositions(joint_model_group);//设一个随机位置
const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");
ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");
//下面进行逆运动学计算，设置尝试次数为10，时间限制为0.1秒。
std::size_t attempts = 10;
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);
//如果得到结果，我们将joint_model_group中保存的结果打印出来。否则报错。
if (found_ik)
{
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]);
}
}
else
{
ROS_INFO("Did not find IK solution");
}
//下面的方法可以获得雅克比矩阵。
Eigen::Vector3d reference_point_position(0.0, 0.0, 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");注意到机器人你运动学计算用的RobotStatePtr中的setFromIK函数，而前文我说它你运动学计算用的是kdl_kinematics_plugin/KDLKinematicsPlugin，他们是怎么联系起来的呢？
下面是kinematics.yaml中的内容，运动学求解器kinematics_solver参数设为了kdl_kinematics_plugin/KDLKinematicsPlugin。至于RobotStatePtr内部是怎么调用到kdl的，不知道，找了半天找不到啊，谁知道怎么找，还请告诉我一下呀，感激不尽。
panda_arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.05
kinematics_solver_attempts: 5总之如果把这个去掉，他是不会计算逆运动学的，报错
No kinematics solver instantiated for group 'panda_arm'然后是规划、约束、碰撞检测，没啥意思，很多参考资料
下面是抓取
先来几张图吧
首先要定义一些基本参数，包括规划场景和规划组的借口
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroupInterface group("panda_arm");
group.setPlanningTime(45.0);//设定规划时间，45秒，长吧然后向环境中添加物体，两个桌子和被抓物体。其中被抓物体的坐标
collision_objects[2].primitive_poses[0].position.x = 0.5;
collision_objects[2].primitive_poses[0].position.y = 0;
collision_objects[2].primitive_poses[0].position.z = 0.5;然后就抓
pick(group);然后就放下
place(group);里面是怎么实现的，不想管了。看源码吧。
OMPL
……
