机器人模型 RobotModel 和机器人状态 RobotState¶
在本节中，我们将引导您逐步了解在 MoveIt 中运动学的 C++ API。
RobotModel 类和 RobotState 类¶
RobotModel 类和 RobotState 类是您使用机器人运动学的两个核心类。
RobotModel 类包含了所有 link 和关节之间的关系，包括从 URDF 加载的关节限位信息。RobotModel 类还将机器人的 link 和关节从 SRDF 文件里定义的 planning group 里分开。关于 URDF 和 SRDF 在 URDF 和 SRDF 教程 里有单独介绍。
RobotState 类包含了特定时间点上和机器人有关的信息，并将关节位置以及可选的速度和加速度储存在一些 vector 里。此信息可用于获取机器人的运动学信息，其由机器人的当前状态决定，例如末端执行器的雅可比行列式。
RobotState 类还包含一些辅助程序，用于根据末端执行器位置（笛卡尔姿态）设置机械臂位置并计算笛卡尔轨迹。
在本示例中，我们将逐步介绍如何将这些类在 Panda 机器人上使用。
运行代码¶
本教程中所有的代码都可从 MoveIt 安装程序里包含的``moveit_tutorials`` 包里编译运行。
使用 roslaunch 启动 launch 文件以直接运行 moveit_tutorials 包里的代码：
roslaunch moveit_tutorials robot_model_and_robot_state_tutorial.launch
预期输出¶
预期的输出将会是以下形式。因为我们使用了随机的关节值，所以数字可能和您看到的有出入：
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 控制台日志。
整个代码¶
整个代码见 MoveIt GitHub project 。
开始¶
设置 RobotModel 类非常容易。 通常，您会发现大多数更高级别的组件都将返回指向 RobotModel 的共享指针。 您应该尽可能使用共享指针。 在此示例中，我们将从此类的共享指针开始，且仅讨论基础的 API 。 您可以查看这些类的 API 源码，以获取更多有关如何使用这些类的所提供的更多功能的信息。
我们将从实例化 RobotModelLoader 对象开始， 该对象将在 ROS 参数服务器上查找 robot description， 并构造一个供我们使用的 RobotModel 类。
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());
使用 RobotModel, 我们可以构建一个保存机器人配置的 RobotState 。 我们将此状态下的所有关节都设置为其默认值。 然后我们可以得到一个 JointModelGroup, 它代表特定 planning group 的机器人模型，例如 Panda 机器人的 “panda_arm” 。
moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
获得关节位置¶
我们可以检索存储在机器人状态（kinematic_state）中的 Panda 机械臂的当前关节位置。
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]);
}
关节限位¶
setJointGroupPositions() 函数本身不会限制关节位置，但是调用 enforceBounds() 函数可以做到这点。
/* 设置 Panda 臂的一个关节位置为超出其限位的值 */
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"));
正向运动学¶
现在，我们可以为一组随机关节位置进行正向运动学计算。 请注意，我们想找到 “panda_link8” 的位姿， 它是机器人 “panda_arm” 这个 planning group 中最远端的连杆 (link) 。
kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Isometry3d& 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");
逆向运动学¶
现在，我们可以解决 Panda 机器人的逆运动学问题（IK）。要解决 IK ，我们将需要以下内容：
末端执行器的所需位姿（默认情况下，末端执行器是 “panda_arm” 连杆链中的最后一个 link）， 即我们在以上步骤中计算得到的 end_effector_state 。
求解超时设置 timeout: 0.1 s
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 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");
}
获得雅克比矩阵¶
我们还能从 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");
Launch 文件¶
要运行代码，您将需要一个包含执行以下两项操作的 launch 文件：
将 Panda 机器人的 URDF 文件和 SRDF 文件加载到参数服务器上；
将由 MoveIt 配置助手生成的 kinematics_solver 配置放到 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>
开源反馈
有地方需要改进？请在 GitHub page 新开一个 pull request。