MoveIt 教程【4】——Robot Model and Robot State

本篇博客，介绍通过C++ API使用 MoveIt 中的运动学。包括正运动学求解、逆运动学求解以及雅可比矩阵求解。

1. RobotModel 和 RobotState 类

RobotModel 和 RobotState是访问机器人运动学的两个核心类。

RobotModel 类包含所有连杆和关节之间的关系，包括从URDF加载的关节限制属性。RobotModel 还将机器人的链接和关节分开到SRDF中定义的规划组中。

RobotState 类包含机器人在某一时刻的信息，存储关节位置、速度、加速度向量等。这些速度和加速度可以用于获得关于机器人的运动信息，这取决于它的当前状态，例如末端执行器的雅可比。

2. 运行示例代码

roslaunch moveit_tutorials robot_model_and_robot_state_tutorial.launch

3. 期望输出

4. 代码

#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();

     start    /

  // 设置开始使用 RobotModel 类是很容易的。在一般情况下，你会发现，
  // 最高级的组件将返回一个共享指针给 RobotModel 类。你应该总是在可能的情况下使用它。
  // 在这个示例中，我们将从这样一个共享指针开始，只讨论基本的API。
  // 您可以查看这些类的实际代码API以获取更多关于如何使用这些类功能的更多信息。
  //
  // 我们开始实例化RobotModelLoader对象，它从ROS参数服务器查找 robot description 、
  // 为我们构建 Robotodel。
  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机器人“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();

       Get Joint Values     ///  获得关节值

  // 我们可以获取保持在状态中 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]);
  }

  ///     Joint Limits     ///  关节限制

  // setJointGroupPositions()函数不能强制自己设置关节限制，但可以通过调用enforceBounds()来设置
  //  设置 Panda arm 规划组中的一个关节超过关节限制
  joint_values[0] = 5.57;
  kinematic_state->setJointGroupPositions(joint_model_group, joint_values);

  // 检查当前关节角是否超过限制
  ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

  // 强制关节限制，再次检查
  kinematic_state->enforceBounds();
  ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

  /    Forward Kinematics       正向运动学

  // 现在，我们可以计算出一组随机关节值的正向运动学。
  // 注意，我们要找到 panda_arm 规划组最远端的连杆 panda_link8 的姿态
  kinematic_state->setToRandomPositions(joint_model_group);
  const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");

  /* 打印末端位姿 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");

  //     Inverse Kinematics     /   逆向运动学
  
  // 我们现在可以解决 panda 机器人的逆向运动学，为了求解 IK，需要以下：
  //  * 末端执行器的期望位姿 end_effector_state 是我们上一步计算得到的
  //  * The 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");
  }

  ///     Get the Jacobian          获得 Jacobian
  
  // 我们可以从 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");

  ros::shutdown();
  return 0;
}

5. Launch 文件

<launch>

<!-- 上传Panda URDF和SRDF到参数服务器 -->
  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>
<!-- 放置运动学求解配置（由配置助手生成的）到ROS参数服务器对应节点的命名空间下，这个节点是教程里实例化的类 -->
  <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>