RobotModel 和 RobotState 类

一、例程

Roslaunch 启动文件以直接从 moveit_tutorials 运行代码：

输出

二、代码

1.我们将首先实例化一个 RobotModelLoader 对象，该对象将在 ROS 参数服务器上查找机器人描述并构造一个 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，它代表特定组的机器人模型，例如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();

检索存储在 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]);
}

2.联合限制

etJointGroupPositions() 本身不会强制执行联合限制，但调用 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"));

3.正向运动学

现在，可以计算一组随机关节值的正向运动学。请注意，我们希望找到“panda_link8”的位姿，它是机器人“panda_arm”组中最远端的链接。

kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Isometry3d& 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");

4.逆运动学

现在可以求解 Panda 机器人的逆运动学 (IK)。要解决 IK，我们将需要以下内容：

末端执行器的所需姿势（默认情况下，这是“panda_arm”链中的最后一个链接）：我们在上述步骤中计算的 end_effector_state。
超时：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");
}

5.获取雅可比行列式

我们还可以从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");

三、启动文件

要运行代码，您需要一个启动文件，它可以做两件事：
将 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>