【MoveIt 2】直接通过 C++ API 使用 MoveIt :机器人模型和机器人状态

最新推荐文章于 2024-09-20 10:55:02 发布

十年一梦实验室

最新推荐文章于 2024-09-20 10:55:02 发布

阅读量842

点赞数 13

文章标签： c++ 机器人开发语言

本文链接：https://blog.csdn.net/cxyhjl/article/details/140965724

版权

直接通过 C++ API 使用 MoveIt

使用 MoveIt 构建更复杂的应用程序通常需要开发人员深入研究 MoveIt 的 C++ API。额外的好处是，直接使用 C++ API 可以跳过许多 ROS 服务/操作层，从而显著提高性能。

机器人模型和机器人状态
规划场景
规划场景监视器
规划场景 ROS API
运动规划 API
运动规划管道
创建 MoveIt 插件
可视化碰撞
时间参数化
近似约束流形的规划
挑选和放置
MoveIt 抓取
MoveIt 深度抓取
子帧
MoveItCpp 教程
使用 Bullet 进行碰撞检测
差动驱动移动底座和机械臂的规划

机器人模型和机器人状态

在本节中，我们将向您介绍在 MoveIt 中使用运动学的 C++ API。

RobotModel 和 RobotState 类

RobotModel 和 RobotState 类是使您能够访问机器人运动学的核心类。

https://github.com/moveit/moveit2/blob/main/moveit_core/robot_model/include/moveit/robot_model/robot_model.h

https://github.com/moveit/moveit2/blob/main/moveit_core/robot_state/include/moveit/robot_state/robot_state.h

RobotModel 类包含所有连杆和关节之间的关系，包括从 URDF 加载的关节限制属性。RobotModel 还将机器人的连杆和关节分为 SRDF 中定义的规划组。可以在此处找到有关 URDF 和 SRDF 的单独教程：URDF 和 SRDF 教程 https://moveit.picknik.ai/main/doc/examples/urdf_srdf/urdf_srdf_tutorial.html

RobotState 包含有关机器人在某个时间点的信息，存储关节位置的向量，并可选地存储速度和加速度。此信息可用于获取依赖于机器人当前状态的运动学信息，例如末端执行器的雅可比矩阵。

RobotState 还包含用于根据末端执行器位置（笛卡尔姿态）设置手臂位置和计算笛卡尔轨迹的辅助函数。

在这个例子中，我们将演示如何使用这些类与 Panda。

入门

如果您还没有这样做，请确保您已完成入门中的步骤。

运行代码

本教程中的所有代码都可以从您作为 MoveIt 设置一部分的 moveit2_tutorials 包中编译和运行。

直接从 moveit2_tutorials 运行代码的启动文件：

ros2 launch moveit2_tutorials robot_model_and_robot_state_tutorial.launch.py

预期输出

预期输出将如下所示。数字将不匹配，因为我们使用的是随机关节值：

... [robot_model_and_state_tutorial]: Model frame: world
... [robot_model_and_state_tutorial]: Joint panda_joint1: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint2: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint3: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint4: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint5: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint6: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint7: 0.000000
... [robot_model_and_state_tutorial]: Current state is not valid
... [robot_model_and_state_tutorial]: Current state is valid
... [robot_model_and_state_tutorial]: Translation:
-0.368232
0.645742
0.752193


... [robot_model_and_state_tutorial]: Rotation:
 0.362374 -0.925408  -0.11093
 0.911735  0.327259  0.248275
 -0.193453 -0.191108  0.962317


... [robot_model_and_state_tutorial]: Joint panda_joint1: 2.263889
... [robot_model_and_state_tutorial]: Joint panda_joint2: 1.004608
... [robot_model_and_state_tutorial]: Joint panda_joint3: -1.125652
... [robot_model_and_state_tutorial]: Joint panda_joint4: -0.278822
... [robot_model_and_state_tutorial]: Joint panda_joint5: -2.150242
... [robot_model_and_state_tutorial]: Joint panda_joint6: 2.274891
... [robot_model_and_state_tutorial]: Joint panda_joint7: -0.774846
... [robot_model_and_state_tutorial]: Jacobian:
  -0.645742     -0.26783   -0.0742358    -0.315413    0.0224927    -0.031807 -2.77556e-17
  -0.368232     0.322474    0.0285092    -0.364197   0.00993438     0.072356  2.77556e-17
          0    -0.732023    -0.109128     0.218716   2.9777e-05     -0.11378 -1.04083e-17
          0    -0.769274    -0.539217     0.640569     -0.36792     -0.91475     -0.11093
          0    -0.638919      0.64923   -0.0973283     0.831769     -0.40402     0.248275
          1  4.89664e-12     0.536419     0.761708     0.415688  -0.00121099     0.962317

注意：如果您的输出具有不同的 ROS 控制台格式，请不要担心。

整个代码

整个代码可以在 MoveIt GitHub 项目中看到。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp

// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h> // 包含MoveIt的机器人模型加载器头文件
#include <moveit/robot_model/robot_model.h> // 包含MoveIt的机器人模型头文件
#include <moveit/robot_state/robot_state.h> // 包含MoveIt的机器人状态头文件


int main(int argc, char** argv)
{
  rclcpp::init(argc, argv); // 初始化ROS2
  rclcpp::NodeOptions node_options; // 创建节点选项对象
  // 这行代码允许加载未声明的参数
  // 最佳实践是应该在相应的类中声明参数并提供预期用途的描述
  node_options.automatically_declare_parameters_from_overrides(true);
  auto node = rclcpp::Node::make_shared("robot_model_and_state_tutorial", node_options); // 创建共享指针节点
  const auto& LOGGER = node->get_logger(); // 获取日志记录器


  // BEGIN_TUTORIAL
  // 开始
  // ^^^^^
  // 设置开始使用RobotModel类非常简单。通常，你会发现大多数高级组件会返回一个指向RobotModel的共享指针。你应该尽可能使用它。在这个例子中，我们将从这样的共享指针开始，并仅讨论基本的API。你可以查看这些类的实际代码API，以获取有关如何使用这些类提供的更多功能的信息。
  //
  // 我们将从实例化一个`RobotModelLoader`对象开始，该对象将查找ROS参数服务器上的机器人描述，并为我们构建一个RobotModel以供使用。
  //
  // .. _RobotModelLoader:
  //     https://github.com/moveit/moveit2/blob/main/moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h
  robot_model_loader::RobotModelLoader robot_model_loader(node); // 实例化RobotModelLoader对象
  const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); // 获取运动学模型
  RCLCPP_INFO(LOGGER, "Model frame: %s", kinematic_model->getModelFrame().c_str()); // 输出模型框架


  // 使用RobotModel，我们可以构建一个RobotState来维护机器人的配置。我们将所有关节设置为默认值。然后我们可以获取一个JointModelGroup，它代表特定组的机器人模型，例如Panda机器人的“panda_arm”。
  moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(kinematic_model)); // 创建RobotState对象
  robot_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;
  robot_state->copyJointGroupPositions(joint_model_group, joint_values); // 复制关节组位置
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]); // 输出每个关节的值
  }


  // 关节限制
  // ^^^^^^^^^^^^
  // setJointGroupPositions()本身不强制执行关节限制，但调用enforceBounds()会执行此操作。
  /* 将Panda手臂中的一个关节设置为超出其关节限制 */
  joint_values[0] = 5.57;
  robot_state->setJointGroupPositions(joint_model_group, joint_values); // 设置关节组位置


  /* 检查是否有任何关节超出其关节限制 */
  RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (robot_state->satisfiesBounds() ? "valid" : "not valid")); // 检查当前状态是否有效


  /* 强制执行此状态的关节限制并再次检查 */
  robot_state->enforceBounds(); // 强制执行关节限制
  RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (robot_state->satisfiesBounds() ? "valid" : "not valid")); // 再次检查当前状态是否有效


  // 正向运动学
  // ^^^^^^^^^^^^^^^^^^
  // 现在，我们可以为一组随机关节值计算正向运动学。注意，我们希望找到“panda_link8”的姿态，它是机器人“panda_arm”组中最远的链接。
  robot_state->setToRandomPositions(joint_model_group); // 设置随机位置
  const Eigen::Isometry3d& end_effector_state = robot_state->getGlobalLinkTransform("panda_link8"); // 获取末端执行器状态


  /* 打印末端执行器姿态。请记住，这是在模型框架中 */
  RCLCPP_INFO_STREAM(LOGGER, "Translation: \n" << end_effector_state.translation() << "\n"); // 输出平移
  RCLCPP_INFO_STREAM(LOGGER, "Rotation: \n" << end_effector_state.rotation() << "\n"); // 输出旋转


  // 逆向运动学
  // ^^^^^^^^^^^^^^^^^^
  // 现在我们可以为Panda机器人求解逆向运动学（IK）。要解决IK，我们需要以下内容：
  //
  //  * 末端执行器的期望姿态（默认情况下，这是“panda_arm”链中的最后一个链接）：我们在上一步中计算的end_effector_state。
  //  * 超时时间：0.1秒
  double timeout = 0.1;
  bool found_ik = robot_state->setFromIK(joint_model_group, end_effector_state, timeout); // 求解逆向运动学


  // 现在，我们可以打印出IK解决方案（如果找到）：
  if (found_ik)
  {
    robot_state->copyJointGroupPositions(joint_model_group, joint_values); // 复制关节组位置
    for (std::size_t i = 0; i < joint_names.size(); ++i)
    {
      RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]); // 输出每个关节的值
    }
  }
  else
  {
    RCLCPP_INFO(LOGGER, "Did not find IK solution"); // 未找到IK解决方案
  }


  // 获取雅可比矩阵
  // ^^^^^^^^^^^^^^^^
  // 我们还可以从RobotState获取雅可比矩阵。
  Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0); // 参考点位置
  Eigen::MatrixXd jacobian;
  robot_state->getJacobian(joint_model_group, robot_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
                           reference_point_position, jacobian); // 获取雅可比矩阵
  RCLCPP_INFO_STREAM(LOGGER, "Jacobian: \n" << jacobian << "\n"); // 输出雅可比矩阵
  // END_TUTORIAL


  rclcpp::shutdown(); // 关闭ROS2
  return 0;
}

开始

开始使用 RobotModel 类非常容易。一般来说，您会发现大多数高级组件将返回一个指向 RobotModel 的共享指针。您应始终在可能的情况下使用它。在此示例中，我们将从这样一个共享指针开始，并仅讨论基本 API。您可以查看这些类的实际代码 API，以获取有关如何使用这些类提供的更多功能的信息。

我们将首先实例化一个 RobotModelLoader 对象，它将在 ROS 参数服务器上查找机器人描述并为我们构建一个 RobotModel 以供使用。

robot_model_loader::RobotModelLoader robot_model_loader(node);
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
RCLCPP_INFO(LOGGER, "Model frame: %s", kinematic_model->getModelFrame().c_str());
// Model frame: world

使用 RobotModel，我们可以构建一个维护机器人配置的 RobotState。我们将把状态中的所有关节设置为默认值。然后，我们可以获得一个 JointModelGroup https://github.com/moveit/moveit2/blob/main/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h，它代表特定组的机器人模型，例如 Panda 机器人的“panda_arm”。

moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(kinematic_model));
robot_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;
robot_state->copyJointGroupPositions(joint_model_group, joint_values);
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
  RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}

关节限制

setJointGroupPositions() 本身不强制执行关节限制，但调用 enforceBounds() 将会执行。

/* Set one joint in the Panda arm outside its joint limit */
joint_values[0] = 5.57;
robot_state->setJointGroupPositions(joint_model_group, joint_values);


/* Check whether any joint is outside its joint limits */
RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (robot_state->satisfiesBounds() ? "valid" : "not valid"));


/* Enforce the joint limits for this state and check again*/
robot_state->enforceBounds();
RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (robot_state->satisfiesBounds() ? "valid" : "not valid"));

正向运动学

现在，我们可以计算一组随机关节值的正向运动学。请注意，我们想要找到“panda_arm”组中最远端的链接“panda_link8”的姿态。

robot_state->setToRandomPositions(joint_model_group);
const Eigen::Isometry3d& end_effector_state = robot_state->getGlobalLinkTransform("panda_link8");


/* Print end-effector pose. Remember that this is in the model frame */
RCLCPP_INFO_STREAM(LOGGER, "Translation: \n" << end_effector_state.translation() << "\n");
RCLCPP_INFO_STREAM(LOGGER, "Rotation: \n" << end_effector_state.rotation() << "\n");

逆运动学

我们现在可以为 Panda 机器人解决逆运动学（IK）问题。要解决 IK，我们需要以下内容：

末端执行器的期望姿态（默认情况下，这是“panda_arm”链中的最后一个连杆）：我们在上一步中计算的 end_effector_state。
超时：0.1 秒

double timeout = 0.1;
bool found_ik = robot_state->setFromIK(joint_model_group, end_effector_state, timeout);

现在，我们可以打印出 IK 解决方案（如果找到的话）：

if (found_ik)
{
  robot_state->copyJointGroupPositions(joint_model_group, joint_values);
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
  }
}
else
{
  RCLCPP_INFO(LOGGER, "Did not find IK solution");
}

获取雅可比矩阵

我们也可以从 RobotState 获取雅可比矩阵。

Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
Eigen::MatrixXd jacobian;
robot_state->getJacobian(joint_model_group, robot_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
                         reference_point_position, jacobian);
RCLCPP_INFO_STREAM(LOGGER, "Jacobian: \n" << jacobian << "\n");

启动文件

要运行代码，您需要一个启动文件，该文件执行两项操作：
- 将 Panda URDF 和 SRDF 加载到参数服务器上，并且
- 将 MoveIt 设置助手生成的 kinematics_solver 配置放置到 ROS 参数服务器上，在实例化本教程中类的节点的命名空间中。

from launch import LaunchDescription # 从launch模块导入LaunchDescription类
from launch_ros.actions import Node # 从launch_ros.actions模块导入Node类
from moveit_configs_utils import MoveItConfigsBuilder # 从moveit_configs_utils模块导入MoveItConfigsBuilder类


def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs() 
    # 使用MoveItConfigsBuilder创建一个MoveIt配置对象，指定机器人资源为“moveit_resources_panda”


    tutorial_node = Node(
        package="moveit2_tutorials", # 指定节点所属的包名为“moveit2_tutorials”
        executable="robot_model_and_robot_state_tutorial", # 指定可执行文件名为“robot_model_and_robot_state_tutorial”
        output="screen", # 将节点的输出设置为屏幕
        parameters=[
            moveit_config.robot_description, # 设置机器人描述参数
            moveit_config.robot_description_semantic, # 设置机器人语义描述参数
            moveit_config.robot_description_kinematics, # 设置机器人运动学描述参数
        ],
    )


    return LaunchDescription([tutorial_node]) # 返回包含该节点的LaunchDescription对象