Ros2 Moveit2 - Robot Model and Robot State

news/2024/11/13 14:02:19/文章来源: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])

 

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.hqwc.cn/news/784516.html

如若内容造成侵权/违法违规/事实不符，请联系编程知识网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

用友crm客户关系管理help.php存在任意文件读取漏洞

用友crm客户关系管理help.php存在任意文件读取漏洞

产品介绍： 用友U8CRM模块是一个综合性的客户关系管理系统，旨在帮助企业从客户出发，以客户关系为管理对象，通过动态管理客户信息、获得 客户知识和评判客户价值状况，来全面提升并保持企业的竞争优势及盈利能力。Fofa语句：body="用友 U8CRM"pocGET /pub/help.php…
阅读更多...
史上最全：大厂P6/P7/P8 的职业技能，薪资水平，成长路线 ，最全的对照表。 （建议 收藏，慢慢查）

史上最全：大厂P6/P7/P8 的职业技能，薪资水平，成长路线 ，最全的对照表。 （建议 收藏，慢慢查）

文章很长，且持续更新，建议收藏起来，慢慢读！疯狂创客圈总目录 博客园版 为您奉上珍贵的学习资源 ： 免费赠送 :《尼恩Java面试宝典》 持续更新+ 史上最全 + 面试必备 2000页+ 面试必备 + 大厂必备 +涨薪必备 免费赠送 :《尼恩技术圣经+高并发系列PDF》 ，帮你 实现技术自由，…
阅读更多...
『模拟赛』暑假集训CSP提高模拟25

『模拟赛』暑假集训CSP提高模拟25

『模拟赛记录』暑假集训CSP提高模拟25Rank 学新东西，不算挂分（确信。A. 可持久化线段树 原板子[SP11470] TTM - To the moon 主席树，不过区间修改。 赛时想到标记永久化了，不过打 pushdown 的时候没新开点，于是 -100pts。 还挺简单的，建树和更改动态开点，按线段树来，加…
阅读更多...
2024暑假集训测试29

2024暑假集训测试29

前言比赛链接。今上午在家打的，下午回的学校，话说啥时候改成 \(7\) 点开始了？ T1 是板子但是没打过标记永久化，想了一段时间想起树状数组维护区间求和操作于是用主席树实现了这个，赛时 T1 不给大样例所以调了挺长时间才放心；T2 想了一会儿没想出来就先打 T3 了，T3 想了一…
阅读更多...
hdu2604

hdu2604

用f(n)表示n个人满足条件的结果，那么如果最后一个人是m的话，那么前n-1个满足条件即可，就是f(n-1)； 如果最后一个是f那么这个还无法推出结果，那么往前再考虑一位：那么后三位可能是：mmf, fmf, mff, fff，其中fff和fmf不满足题意所以我们不考虑，但是如果是 mmf的话那么前n…
阅读更多...
半导体器件设计概述

半导体器件设计概述

功率半导体体器件简要概述功率半导体的内涵 功率半导体是一类能够在高电压、高电流条件下工作的半导体器件，主要用于开关、控制和转换电能，功率半导体的基本原理可归纳为以下三个方面：截至特性：当功率半导体截至时，其内部的电阻变得很大，从而阻止电流通过，起到开关的作用…
阅读更多...
2、coredump文件的配置

2、coredump文件的配置

1、开启core文件的生成 使用ulimit -a命令查看是否打开core文件，如果大小为0，则表明没有打开，如果非0，则表明打开，本人虚拟机上已经设置为unlimited，所以已经打开（1）临时打开 使用ulimit -c命令打开，如设置为无限制，则使用ulimit -c unlimited （2）永久打开 sudo vi…
阅读更多...
feehicms 文件上传

feehicms 文件上传

如果文件上传中出现了奇奇怪怪的参数可以试着查看其来源和作用侵权声明 本文章中的所有内容（包括但不限于文字、图像和其他媒体）仅供教育和参考目的。如果在本文章中使用了任何受版权保护的材料，我们满怀敬意地承认该内容的版权归原作者所有。 如果您是版权持有人，并且认为…
阅读更多...
如何用纯CSS绘制三角形

如何用纯CSS绘制三角形

要用纯CSS绘制三角形，可以通过利用CSS中的border属性来实现。具体来说，核心原理是通过设置不同方向的边框颜色和宽度来形成三角形效果。以下是写技术博客时可以介绍的几个要点： 1. 三角形的基本原理 CSS三角形的关键在于设置一个元素的宽高为0，同时通过边框来创建三角形。例…
阅读更多...
QTabWidget自定义样式(仿DotNetBar)

QTabWidget自定义样式(仿DotNetBar)

QSS如下，若需要tab栏背景色需要添加ui->tabWidget->setAttribute(Qt::WA_StyledBackground); 语句使background-color生效，这个时候qtdesigner中仍然看不到背景色，但是不要担心它是生效的，只需在属性中勾上autofillbackground即可在designer中预览（该属性在QWidget属…
阅读更多...
如何用纯CSS绘制三角形--02

如何用纯CSS绘制三角形--02

通过结合@keyframes动画，让三角形实现旋转、移动等动态效果代码如下：<!DOCTYPE html> <html lang="en"> <head><meta charset="UTF-8"><meta name="viewport" content="width=device-width, initial-scale=1.…
阅读更多...
Kettle 9 加密与解密

Kettle 9 加密与解密

使用kettle 加密和解密 ，测试环境： win10 + PDI 9.0 (Pentaho Data integation 9.0.0.0-423 ) 1、进入KETTLE的安装目录 如：cd /d E:\DATA-INTERGRATION 命令行执行： encr.bat -kettle 123 可生成 Encrypted 2be98afc86aa7f2e4cb79ce10bec3fd89 ，即为123对应KETTLE生成…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023