在本节中，将了解在MoveIt中使用kinematics的C++ API。
一.The RobotModel and RobotState Classes
RobotModel和RobotState类是允许访问rebot kinematics的核心类。
RobotModel类包含所有links和joints之间的关系，包括从URDF加载的joint限制属性。RobotModel还将robot的links和joints分成SRDF中定义的planning groups。在这里可以找到关于URDF和SRDF的单独教程：URDF和SRDF教程 RobotState包含关于robot的实时快照信息，存储joint位置和可选的速度和加速度向量，可用于获得关于robot的运动学信息，这些信息取决于robot的当前状态，比如末端执行器的雅可比矩阵。
RobotState还包含辅助功能，用于基于末端执行器位置[笛卡尔位姿]设置手臂位置和计算笛卡尔轨迹。
在本例中，将遍历使用Panda使用这些类的过程。
二.Running the Code
本教程中的所有代码都可以从moveit_tutorials包[MoveIt的一部分]编译并运行设置。
Roslaunch启动文件运行代码直接从moveit_tutorials：
roslaunch moveit_tutorials robot_model_and_robot_state_tutorial.launch三.Expected Output
预期的输出形式如下。这些数字将不匹配，因为使用随机joint值：
ros.moveit_tutorials: Model frame: /panda_link0
ros.moveit_tutorials: Joint panda_joint1: 0.000000
ros.moveit_tutorials: Joint panda_joint2: 0.000000
ros.moveit_tutorials: Joint panda_joint3: 0.000000
ros.moveit_tutorials: Joint panda_joint4: 0.000000
ros.moveit_tutorials: Joint panda_joint5: 0.000000
ros.moveit_tutorials: Joint panda_joint6: 0.000000
ros.moveit_tutorials: Joint panda_joint7: 0.000000
ros.moveit_tutorials: Current state is not valid
ros.moveit_tutorials: Current state is valid
ros.moveit_tutorials: Translation:
-0.541498
-0.592805
0.400443
ros.moveit_tutorials: Rotation:
-0.395039 0.600666 -0.695086
0.299981 -0.630807 -0.715607
-0.868306 -0.491205 0.0690048
ros.moveit_tutorials: Joint panda_joint1: -2.407308
ros.moveit_tutorials: Joint panda_joint2: 1.555370
ros.moveit_tutorials: Joint panda_joint3: -2.102171
ros.moveit_tutorials: Joint panda_joint4: -0.011156
ros.moveit_tutorials: Joint panda_joint5: 1.100545
ros.moveit_tutorials: Joint panda_joint6: 3.230793
ros.moveit_tutorials: Joint panda_joint7: -2.651568
ros.moveit_tutorials: Jacobian:
0.592805 -0.0500638 -0.036041 0.366761 -0.0334361 0.128712 -4.33681e-18
-0.541498 -0.0451907 0.0417049 -0.231187 0.0403683 0.00288573 3.46945e-18
0 -0.799172 0.0772022 -0.247151 0.0818336 0.0511662 0
0 0.670056 -0.742222 0.349402 -0.748556 -0.344057 -0.695086
0 -0.74231 -0.669976 -0.367232 -0.662737 0.415389 -0.715607
1 4.89669e-12 0.0154256 0.862009 0.021077 0.842067 0.0690048注意：如果输出具有不同的ROS控制台格式，请不要担心。可以通过阅读这篇博客文章来定制ROS控制台日志记录器。
四.The Entire Code
全部代码参考文献[2]。
1.Start
设置开始使用RobotModel类非常简单。通常，会发现大多数高级组件将返回一个指向RobotModel的共享指针。应该尽可能地使用它。在本例中，将从这样一个共享指针开始，只讨论基本API。可以查看这些类的实际代码API，以获得关于如何使用这些类提供的更多特性的更多信息。
将从实例化一个RobotModelLoader对象开始，该对象将在ROS参数服务器上查找robot描述，并构造一个供使用的RobotModel。
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());使用RobotModel，可以构建一个RobotState来维持robot的配置。将把状态中的所有joints设置为它们的默认值。然后可以得到一个JointModelGroup，它代表一个特定组的robot模型，例如，Panda robot的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();2.Get Joint Values
可以为Panda arm检索存储在状态中的当前joint值集。
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]);
}3.Joint Limits
setJointGroupPositions()本身不执行joint限制，但调用enforceBounds()将执行它。
/* Set one joint in the Panda arm outside its joint limit */
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"));4.Forward Kinematics
现在，可以计算一组随机joint值的正运动学。请注意，希望找到panda_link8的位置，它是机器人panda_arm组中最远端的链接。
kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");
/* Print end-effector pose. Remember that this is in the model frame */
ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");5.Inverse Kinematics
现在可以求解Panda robot的逆运动学。要解决IK，需要以下几点：
[1]所需的末端执行器姿态[默认情况下，这是panda_arm链中的最后一个链接]：end_effector_state，在上面的步骤中计算过。
[2]解决IK的尝试次数：10
[3]每次尝试的超时时间：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);现在，可以打印出IK解决方案[如果找到]：
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");
}6.Get the Jacobian
还可以从RobotState得到雅可比矩阵。
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");7.The Launch File
运行代码，需要一个launch文件做两件事：
[1]将Panda URDF和SRDF加载到参数服务器中
[2]放置由MoveIt生成的kinematics_solver配置，Setup Assistant到ROS参数服务器，该服务器位于本教程中实例化类的节点的名称空间中。
<launch>
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<node name="robot_model_and_robot_state_tutorial"
pkg="moveit_tutorials"
type="robot_model_and_robot_state_tutorial"
respawn="false" output="screen">
<rosparam command="load"
file="$(find panda_moveit_config)/config/kinematics.yaml"/>
</node>
</launch>参考文献：
[1]Robot Model and Robot State：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html
[2]robot_model_and_robot_state_tutorial：https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp
[3]MoveIt!入门教程-目录：https://www.ncnynl.com/archives/201610/947.html
