RobotModel和RobotState类
该RobotModel和RobotState类是核心类，让你访问一个机器人的运动。
该RobotModel类包含的各个环节和关节，包括从URDF加载它们的联合极限性质之间的关系。RobotModel还将机器人的链接和关节分离为SRDF中定义的计划组。有关URDF和SRDF的单独教程可以在这里找到：URDF和SRDF教程
所述RobotState包含在时间上的快照有关机器人的信息，存储的关节位置和任选速度和可以用于获得关于依赖于它的当前状态，诸如末端执行器的雅可比机器人运动学信息的加速度矢量。
RobotState还包含辅助函数，用于根据末端执行器位置（笛卡尔姿势）设置手臂位置，以及计算笛卡尔轨迹。
#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>
int main(int argc, char** argv)
{
ros::init(argc, argv, "robot_model_and_robot_state_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
// http://docs.ros.org/kinetic/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
// 设置开始使用RobotModel类非常简单。通常，您会发现大多数更高级别的组件将返回一个指向RobotModel的共享指针。你应该尽可能使用它。
// 在这个例子中，我们将从这样的共享指针开始，并仅讨论基本API。您可以查看这些类的实际代码API，以获取有关如何使用这些类提供的更多功能的更多信息。
// 我们将首先实例化一个 RobotModelLoader 对象，该对象将在ROS参数服务器上查找机器人描述并构造一个 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。我们将状态中的所有关节设置为其默认值。
// 然后我们可以获得一个 JointModelGroup，它代表特定组的机器人模型，例如熊猫机器人的“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 Limits
// ^^^^^^^^^^^^
// setJointGroupPositions() does not enforce joint limits by itself, but a call to enforceBounds() will do it.
/* Set one joint in the Panda arm outside its joint limit */
// setJointGroupPositions（）本身不强制执行关节限制，但是对enforceBounds（）的调用将执行此操作。
/*将熊猫臂中的一个关节设置在其关节限制之外*/
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"));
// Forward Kinematics
// 正向运动学
//现在，我们可以计算一组随机关节值的正向运动学。请注意，我们希望找到“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");
// 反向运动学
// 我们现在可以解决熊猫机器人的逆运动学（IK）。要解决IK，我们需要以下内容：
// 末端效应器的所需姿势（默认情况下，这是“panda_arm”链中的最后一个链接）：我们在上面的步骤中计算的end_effector_state。
std::size_t attempts = 10;// 在解决IK时尝试的次数：10
double timeout = 0.1;// 每次尝试的超时：0.1秒
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);
//现在，我们可以打印出IK解决方案（如果找到）：
// Now, we can print out the IK solution (if found):
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");
}
// Get the Jacobian
// ^^^^^^^^^^^^^^^^
// We can also get the Jacobian from the :moveit_core:`RobotState`.
//得到雅可比矩阵
//我们也可以从RobotState获得Jacobian 。
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");
// END_TUTORIAL
ros::shutdown();
return 0;
}