Ros2 Moveit2 - Robot Model and Robot State

news/2024/9/21 4:59:36/文章来源:https://www.cnblogs.com/ai-ldj/p/18370396

Robot Model and Robot State

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

RobotModel 和 RobotState 类

RobotModel 和 RobotState 类是提供对机器人运动学访问权限的核心类。

RobotModel 类包含所有链接和关节之间的关系，包括从 URDF 加载的关节限制属性。RobotModel 还将机器人的连杆和关节划分为 SRDF 中定义的规划组。可以在此处找到有关 URDF 和 SRDF 的单独教程：URDF 和 SRDF 教程。

RobotState 包含有关机器人在特定时间点的信息，存储关节位置的向量以及可选的速度和加速度。此信息可用于获取有关机器人的运动学信息，该信息取决于机器人的当前状态，例如末端执行器的雅可比式。

RobotState 还包含帮助程序函数，用于根据末端执行器位置（笛卡尔姿势）设置手臂位置，以及计算笛卡尔轨迹。

在此示例中，我们将演示将这些类与 Panda 一起使用的过程。

开始

如果尚未执行此操作，请确保已完成“入门”中的步骤。

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

Roslaunch 启动文件以直接从 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.110930.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-170    -0.732023    -0.109128     0.218716   2.9777e-05     -0.11378 -1.04083e-170    -0.769274    -0.539217     0.640569     -0.36792     -0.91475     -0.110930    -0.638919      0.64923   -0.0973283     0.831769     -0.40402     0.2482751  4.89664e-12     0.536419     0.761708     0.415688  -0.00121099     0.962317

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

整个代码

完整的代码可以在 MoveIt GitHub 项目中查看。

开始

设置开始使用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());

使用 RobotModel，我们可以构造一个 RobotState，该维护机器人的配置。我们将把处于该状态的所有关节设置为其默认值。然后我们可以获取一个 JointModelGroup，它代表特定群体的机器人模型，例如熊猫机器人的“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"));

enforceBounds 函数是运动规划的一个重要环节，用于保证生成的运动规划是安全的。
这个函数的作用是检查和修正机器人状态，而不是直接控制机器人电机。
真正控制机器人运动的是机器人控制器，它会根据规划好的路径和传感器反馈来驱动电机。

正向运动学

现在，我们可以计算一组随机关节的前向运动学值。请注意，我们想找到“panda_link8” 的姿势，这是机器人的“panda_arm”组。

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 Setup Assistant 生成的kinematics_solver配置放到实例化本教程中类的节点命名空间中的 ROS 参数服务器上。

from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilderdef generate_launch_description():moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()tutorial_node = Node(package="moveit2_tutorials",executable="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])