机器人模型和机器人状态

在本节教程中，将会引导您了解MoveIt2中使用运动学的C++ API。

RobotModel和RobotState类

RobotModel和RobotState类是MoveIt2的核心类，可以让用户访问机器人运动学。

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

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

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

在本教程示例中，将会逐步引导用户了解这些类在Panda机器人上使用的过程。

运行代码

本教程中的所有代码都可以从作为MoveIt2安装组成部分之一的moveit2_tutorials软件包中编译和运行。

使用ros2 launch命令直接从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控制台格式，请不必担心。

完整代码

启动

使用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机器人的“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)
{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;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);/* Check whether any joint is outside its joint limits */
RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));/* Enforce the joint limits for this state and check again*/
kinematic_state->enforceBounds();
RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

正运动学

现在，我们可以计算一组随机关节值的正运动学（FK）。请注意，这里我们想找到机器人“panda_arm”规划组中最远端链接“panda_link8”的位姿。

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 */
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.1s

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){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;
kinematic_state->getJacobian(joint_model_group,kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),reference_point_position, jacobian);
RCLCPP_INFO_STREAM(LOGGER, "Jacobian: \n" << jacobian << "\n");

launch文件

要运行该代码，需要一个启动文件，该启动文件要完成以下两件事：

将Panda机器人的URDF和SRDF加载到参数服务器上；

将MoveIt设置助手生成的kinematics_solver配置推送到ROS 参数服务器上实例化了本教程中各个类的节点命名空间中。

import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
import xacrodef load_file(package_name, file_path):package_path = get_package_share_directory(package_name)absolute_file_path = os.path.join(package_path, file_path)try:with open(absolute_file_path, "r") as file:return file.read()except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where availablereturn Nonedef load_yaml(package_name, file_path):package_path = get_package_share_directory(package_name)absolute_file_path = os.path.join(package_path, file_path)try:with open(absolute_file_path, "r") as file:return yaml.safe_load(file)except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where availablereturn Nonedef generate_launch_description():# planning_contextrobot_description_config = load_file("moveit_resources_panda_description", "urdf/panda.urdf")robot_description_semantic_config = load_file("moveit_resources_panda_moveit_config", "config/panda.srdf")kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config", "config/kinematics.yaml")tutorial_node = Node(package="moveit2_tutorials",executable="robot_model_and_robot_state_tutorial",output="screen",parameters=[{"robot_description": robot_description_config},{"robot_description_semantic": robot_description_semantic_config},kinematics_yaml,],)return LaunchDescription([tutorial_node])

MoveIt2——4.机器人模型和机器人状态相关推荐

V-rep学习笔记：机器人模型创建4—定义模型
完成之前的操作后终于来到最后一步--定义模型,即将之前创建的几何体.关节等元素按层级关系组织成为一个整体. 将最后一个连杆robot_link_dyn6拖放到相应的关节(robot_joint6)下, ...
ROS探索总结（四）（五）（六）——简单的机器人仿真创建简单的机器人模型smartcar 使用smartcar进行仿真
ROS探索总结(四)--简单的机器人仿真前边我们已经介绍了ROS的基本情况,以及新手入门ROS的初级教程,现在就要真正的使用ROS进入机器人世界了.接下来我们涉及到的很多例程都是<ROS by ...
ROS 机器人模型节点的运动控制原理
一般我们启动机器人模型是通过以下的.launch文件启动,如下所示: <launch><param name="robot_description" comman ...
Rviz中控制机器人模型运动（arbotix）
前言: 通过 URDF 结合 rviz 可以创建并显示机器人模型,不过,当前实现的只是静态模型,如何控制模型的运动呢?在此,可以调用 Arbotix 实现此功能. 简介: Arbotix:Arboti ...
ROS——UDRF机器人模型优化（xacro）
文章目录 UDRF机器人模型的不足 UDRF机器人模型的优化实现 xacro模型优势 xacro的使用方法 xacro创建一个小车机器人编辑xacro机器人模型启动模型在rviz中显示小结参考 ...
ROS学习笔记6 URDF建立机器人模型（以双轮差速小车为例）
URDF建立机器人模型一.准备工作一.搭建小车底盘模型 1.小车底盘 2.左右驱动轮 3.前后万向轮 4.launch启动文件 5.效果展示二.摄像头仿真模型 1.添加摄像头模型 2.launc ...
扩散模型加持下，机器人模型DALL-E-Bot可以轻松完成自主重新排列任务
原文链接:https://www.techbeat.net/article-info?id=4217 作者:seven_ 最近,图像扩散模型的发展可谓是风生水起,很多AI大厂纷纷在这一领域展开研究,例 ...
[论文]基于强化学习的无模型水下机器人深度控制
基于强化学习的无模型水下机器人深度控制摘要介绍问题公式 A.水下机器人的坐标框架 B.深度控制问题马尔科夫模型 A.马尔科夫决策 B.恒定深度控制MDP C.弯曲深度控制MDP D.海底追踪的 ...
6.5 Rviz中控制机器人模型运动
ROS入门 6.5 Rviz中控制机器人模型运动 <ROS入门-理论与实践>视频教程镇楼通过 URDF 结合 rviz 可以创建并显示机器人模型,不过,当前实现的只是静态模型,如何控制模 ...
搭建机器人模型进行slam过程记录
有需要的小伙伴可以在评论区留言获取完整的工作空间代码包环境:ubuntu18.04, ROS melodic 机器人底盘URDF文件编写首先在工作空间目录下创建一个功能包,用来放置机器人模型,以及 ...

MoveIt2——4.机器人模型和机器人状态