【ROS-MoveIt！源码学习】ROS中机器人模型的构建（Build RobotModel）

gpeng832

分类：ROS

发布时间 2021.05.07阅读数 4384 评论数 0

1.本文讨论的内容和目的

本文从ROS MOVEIT!的入门教程moveit_tutorial中的运动学模型（kinematic_model_tutorial.launch.cpp）入手，目的是想搞明白：
（1）ROS是如何导入机器人模型；（本文内容）

（2）如何调用逆运动学算法求解。（以后再写）

搞明白这两件事后，就能够自己定义一个机械臂，然后调用自己编写的逆运动学算法。

2.ROS中的机器人模型表示方法

首先说一下ROS中机器人形式，这里只说移动机器人和机械臂两种形式，对移动机器人的基本要求是其整体能够从一个状态运动到另一个状态，而对机械臂的基本要求是希望机械臂的末端从一个状态移动到另一个状态，在运动过程中同时使移动机器人和机械臂满足一些运动学和动力学约束等。如果希望机械臂能够避障的话，还需要采用运动规划（Motion Planning）的策略。

2.1 ROS如何处理复杂的机器人模型

如果用户的机器人CAD模型只有一只机械臂的话（例如LWR机械臂），那么问题就很简单：针对机械臂所有的关节（7个），给定一个搬运的任务，调用现有的逆解算法就可以了。但是如果机器人的模型是类似于PR2机器人的形式，问题就比较复杂了，因为PR2机器人包含有两只手臂，两个手爪，底座是一个小车，一共有80多个关节，既有移动机器人部分也有机械臂部分。在对PR2设计任务的时候，可能在某一时刻只需要对右臂进行规划，也就是只需考虑右臂的7个转动关节。所以这个时候就体现出了SRDF文件机制的优越性了，SRDF文件中使用了将多个的关节重组为若干个关节组合这样的策略，这样就可以对特定的关节组合进行规划，例如将PR2的右臂共7个关节组成名称为“right_arm”的关节组和。

图1 LWR的简化模型

图2 pr2机器人

2.2. ROS中的URDF文件和SRDF文件

刚才提到了SRDF文件，其实在ROS中，构建机器人的模型需要将URDF文件和SRDF配合使用，ROS可以通过launch文件将PR2的URDF文件和SRDF文件上传到ROS参数服务器中，然后就可以实例化RobotModelLoader，从ROS参数服务器中查找名为“robot_description”的PR2机器人模型。然后实例化后的RobotModel可以调用特定的关节组合进行规划。

3. 编写节点显示pr2机器人模型的信息

现在假设不知道moveit_tutorial提供的pr2机器人模型的具体情况（moveit_tutorial的链接），当然我们可以通过查看urdf文件和srdf文件，但是这里我通过编写程序来查看。程序的逻辑大概可以表示为：

3.1 launch文件

launch文件（kinematic_model_tutorial.launch）的内容为：

<launch>
  <include file="$(find pr2_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <node name="kinematic_model_tutorial"
       pkg="moveit_tutorials"
        type="kinematic_model_tutorial"
      respawn="false" output="screen">
   <rosparam command="load" file="$(find pr2_moveit_config)/config/kinematics.yaml"/>
  </node>
</launch>

其中第2-4行命令表示打开另一个launch文件（planning_contex.launch），内容如下：

<launch>
 <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
 <arg name="load_robot_description" default="false"/>

  <!-- Load universal robotic description format (URDF) -->
  <param if="$(arg load_robot_description)" name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'" />
     
  <!-- The semantic description that corresponds to the URDF -->
  <param name="robot_description_semantic" textfile="$(find pr2_moveit_config)/config/pr2.srdf" />

  <!-- Load to the parameter server yaml files -->
  <group ns="robot_description_planning">    
    <rosparam command="load" file="$(find pr2_moveit_config)/config/joint_limits.yaml"/>
  </group>

  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
 <group ns="robot_description_kinematics">
   <rosparam command="load" file="$(find pr2_moveit_config)/config/kinematics.yaml"/>
  </group>
 
</launch>

其中第3行和第5行命令的意义是把pr2的URDF和SRDF上传到了ROS 的参数服务器中，将pr2.urdf的名称命名为"robot_description"，将pr2.srdf命名为"robot_description_semantic"。需要注意的是第3行命令，意义是将xacro文件转化为了urdf文件后上传到参数服务器中，这是ROS里推荐的方法。想要把ROS里的机制搞明白，launch文件的语法、URDF文件的语法、SRDF文件的语法、XACRO文件的语法是必须要掌握的。

3.2 读取ROS的参数服务器的所有参数

在终端输入指令“rosparam list”，会显示出所有的参数，结果如下，可以看到第一个就是机器人模型的参数"robot_description"。

/robot_description
/robot_description_kinematics/left_arm/kinematics_solver
/robot_description_kinematics/left_arm/kinematics_solver_attempts
/robot_description_kinematics/left_arm/kinematics_solver_search_resolution
/robot_description_kinematics/left_arm/kinematics_solver_timeout
/robot_description_kinematics/left_arm_and_torso/kinematics_solver
/robot_description_kinematics/left_arm_and_torso/kinematics_solver_attempts
/robot_description_kinematics/left_arm_and_torso/kinematics_solver_search_resolution
/robot_description_kinematics/left_arm_and_torso/kinematics_solver_timeout
/robot_description_kinematics/right_arm/kinematics_solver
/robot_description_kinematics/right_arm/kinematics_solver_attempts
/robot_description_kinematics/right_arm/kinematics_solver_search_resolution
/robot_description_kinematics/right_arm/kinematics_solver_timeout
/robot_description_kinematics/right_arm_and_torso/kinematics_solver
/robot_description_kinematics/right_arm_and_torso/kinematics_solver_attempts
/robot_description_kinematics/right_arm_and_torso/kinematics_solver_search_resolution
/robot_description_kinematics/right_arm_and_torso/kinematics_solver_timeout
/robot_description_planning/joint_limits/l_elbow_flex_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_elbow_flex_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_elbow_flex_joint/max_acceleration
/robot_description_planning/joint_limits/l_elbow_flex_joint/max_velocity
/robot_description_planning/joint_limits/l_forearm_roll_joint/angle_wraparound
/robot_description_planning/joint_limits/l_forearm_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_forearm_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_forearm_roll_joint/max_acceleration
/robot_description_planning/joint_limits/l_forearm_roll_joint/max_velocity
/robot_description_planning/joint_limits/l_shoulder_lift_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_shoulder_lift_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_shoulder_lift_joint/max_acceleration
/robot_description_planning/joint_limits/l_shoulder_lift_joint/max_velocity
/robot_description_planning/joint_limits/l_shoulder_pan_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_shoulder_pan_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_shoulder_pan_joint/max_acceleration
/robot_description_planning/joint_limits/l_shoulder_pan_joint/max_velocity
/robot_description_planning/joint_limits/l_upper_arm_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_upper_arm_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_upper_arm_roll_joint/max_acceleration
/robot_description_planning/joint_limits/l_upper_arm_roll_joint/max_velocity
/robot_description_planning/joint_limits/l_wrist_flex_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_wrist_flex_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_wrist_flex_joint/max_acceleration
/robot_description_planning/joint_limits/l_wrist_flex_joint/max_velocity
/robot_description_planning/joint_limits/l_wrist_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_wrist_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_wrist_roll_joint/max_acceleration
/robot_description_planning/joint_limits/l_wrist_roll_joint/max_velocity
/robot_description_planning/joint_limits/r_elbow_flex_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_elbow_flex_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_elbow_flex_joint/max_acceleration
/robot_description_planning/joint_limits/r_elbow_flex_joint/max_velocity
/robot_description_planning/joint_limits/r_forearm_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_forearm_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_forearm_roll_joint/max_acceleration
/robot_description_planning/joint_limits/r_forearm_roll_joint/max_velocity
/robot_description_planning/joint_limits/r_shoulder_lift_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_shoulder_lift_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_shoulder_lift_joint/max_acceleration
/robot_description_planning/joint_limits/r_shoulder_lift_joint/max_velocity
/robot_description_planning/joint_limits/r_shoulder_pan_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_shoulder_pan_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_shoulder_pan_joint/max_acceleration
/robot_description_planning/joint_limits/r_shoulder_pan_joint/max_velocity
/robot_description_planning/joint_limits/r_upper_arm_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_upper_arm_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_upper_arm_roll_joint/max_acceleration
/robot_description_planning/joint_limits/r_upper_arm_roll_joint/max_velocity
/robot_description_planning/joint_limits/r_wrist_flex_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_wrist_flex_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_wrist_flex_joint/max_acceleration
/robot_description_planning/joint_limits/r_wrist_flex_joint/max_velocity
/robot_description_planning/joint_limits/r_wrist_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_wrist_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_wrist_roll_joint/max_acceleration
/robot_description_planning/joint_limits/r_wrist_roll_joint/max_velocity
/robot_description_semantic
/rosdistro
/roslaunch/uris/host_robotic_ideapad_y470__40618
/rosversion
/run_id

3.3 编写节点程序

为了演示ROS如何构建机器人模型，对节点kinematic_model_tutorial.launch.cpp进行了一些增删，去掉了对各个关节的极限设置，正运动学和逆运动学的内容，这些以后有机会再写。

#include <ros/ros.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
 
#include <vector>
#include <string>
 
int main(int argc, char **argv)
{
  ros::init (argc, argv, "right_arm_kinematics");
  ros::AsyncSpinner spinner(1);
  spinner.start();
  
  //实例化RobotModelLoader，从ROS参数服务器中查找名为“robot_description”的机器人模型
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  std::cout<<"robot_model_loader done!"<<std::endl;
  robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
  
   //列出pr2机器人中所有的关节模型的名称和关节类型
  std::cout<<"----------Get All JointModels(Name and Type)----------"<<std::endl;
  for(size_t i=0;i<kinematic_model->getJointModels().size();++i)
  {
    std::cout<<i+1<<" : Joint model Name: "<<kinematic_model->getJointModels()[i]->getName()<<std::endl;
    std::cout<<"Joint model Type: "<<kinematic_model->getJointModels()[i]->getTypeName()<<std::endl;
  }
  std::cout<<std::endl;
   
   //列出所有的关节组合的名称
  std::cout<<"----------Get All Jointmodelgroups(Names)----------"<<std::endl;
  for(size_t i=0;i<kinematic_model->getJointModelGroupNames().size();++i)
  {
    std::cout<<i+1<<" JointModelGroup"<<": "<<kinematic_model->getJointModelGroupNames()[i]<<std::endl;
  }
  std::cout<<std::endl;
  
   //列出关节组合“right_arm”中的所有的关节模型的名称和关节类型
  std::cout<<"----------Get right_arm's Jointmodel(Names and Type) ----------"<<std::endl;
  const robot_state::JointModelGroup* right_arm_joint_model_group = kinematic_model->getJointModelGroup("right_arm");
  for(size_t i=0;i<right_arm_joint_model_group->getJointModelNames().size();++i)
  {
    std::cout<<i+1<<" Name: "<<right_arm_joint_model_group->getJointModelNames()[i]<<std::endl;
    std::cout<<"Type: "<<right_arm_joint_model_group->getJointModels()[i]->getTypeName()<<std::endl;
  }
  std::cout<<std::endl;
 
 
  ros::shutdown();
  return 0;
}

在终端输入命令：

roslaunch moveit_tutorials kinematic_model_tutorial.launch

运行结果为：

[ INFO] [1496657572.700761585]: Loading robot model 'pr2'...
[ WARN] [1496657572.716620179]: The root link base_footprint has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
robot_model_loader done!
----------Get All JointModels(Name and Type)----------
1 : Joint model Name: world_joint
Joint model Type: Planar
2 : Joint model Name: base_footprint_joint
Joint model Type: Fixed
………………
87 : Joint model Name: r_torso_lift_side_plate_joint
Joint model Type: Fixed
88 : Joint model Name: torso_lift_motor_screw_joint
Joint model Type: Revolute

----------Get All Jointmodelgroups(Names)----------
1 JointModelGroup: arms
2 JointModelGroup: base
3 JointModelGroup: head
4 JointModelGroup: left_arm
5 JointModelGroup: left_arm_and_torso
6 JointModelGroup: left_gripper
7 JointModelGroup: right_arm
8 JointModelGroup: right_arm_and_torso
9 JointModelGroup: right_gripper
10 JointModelGroup: torso
11 JointModelGroup: whole_body

----------Get right_arm's Jointmodel(Names and Type) ----------
1 Name: r_shoulder_pan_joint
Type: Revolute
2 Name: r_shoulder_lift_joint
Type: Revolute
3 Name: r_upper_arm_roll_joint
Type: Revolute
4 Name: r_upper_arm_joint
Type: Fixed
5 Name: r_elbow_flex_joint
Type: Revolute
6 Name: r_forearm_roll_joint
Type: Revolute
7 Name: r_forearm_joint
Type: Fixed
8 Name: r_wrist_flex_joint
Type: Revolute
9 Name: r_wrist_roll_joint
Type: Revolute
[kinematic_model_tutorial-2] process has finished cleanly
log file: /home/gpeng/.ros/log/84904d0c-49d7-11e7-9538-74e50b6fd36e/kinematic_model_tutorial-2*.log

结果显示了moveit_tutorial提供的pr2.urdf机器人共有88个关节模型，11个关节组合（包括“right_arm”）,"right_arm"组合中包含9个关节，其中2个是fixed的，7个是revolute，猜测只有7个revolute的关节参与了规划。

ROS URDF MoveIt 建模仿真

打赏 0

上一篇：【ROS学习】Solidworks模型转化为URDF文件格式+三连杆机械臂示例+逆运动学

下一篇：【机器人学】机器人开源项目KDL源码学习：（9）KDL中的内联函数

【ROS-MoveIt！源码学习】ROS中机器人模型的构建（Build RobotModel）

gpeng832

为你推荐

ROS系统学习10---利用OpenMP库进行CPU的多线程加速

机器人导航(仿真)-02

精选cartographer与ros环境开发经验技巧总结（持续更新）

ROS机器人应用（2）——SSH登录

树莓派 ROS Noetic学习（三）通过两轮机器人学习 Rviz 和 Gazebo

基于ROS的两轮应急巡检车系列（三）两轮差速机器人轮式里程计编写

关于作者

gpeng832

14

0

0

1

【机器人学】机器人开源项目KDL源码学习：（4）机械臂逆动力学的牛顿欧拉算法

【机器人学】机器人开源项目KDL源码学习：（5）KDL如何求解几何雅克比矩阵

【机器人学】机器人开源项目KDL源码学习：（1）下载源码并在ubuntu下运行geometry部分（旋转矩阵）

相关推荐

ROS入门学习记录（一）树莓派4B安装ROS+远程登录

ROS2时间同步(python)

ROS学习记录（一） Plugin插件

ROS1结合自动驾驶数据集Kitti开发教程(四)画出自己车子模型以及照相机视野

Ubuntu助手 -- 一键自动安装软件，一键进行系统配置

从零开始搭二维激光SLAM --- 基于Hector的栅格地图的构建

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

【ROS-MoveIt！源码学习】ROS中机器人模型的构建（Build RobotModel）

gpeng832

为你推荐

ROS系统学习10---利用OpenMP库进行CPU的多线程加速

机器人导航(仿真)-02

精选cartographer与ros环境开发经验技巧总结（持续更新）

ROS机器人应用（2）——SSH登录

树莓派 ROS Noetic学习（三）通过两轮机器人学习 Rviz 和 Gazebo

基于ROS的两轮应急巡检车系列（三）两轮差速机器人轮式里程计编写

评论（0）

关于作者

gpeng832

14

0

0

1

【机器人学】机器人开源项目KDL源码学习：（4）机械臂逆动力学的牛顿欧拉算法

【机器人学】机器人开源项目KDL源码学习：（5）KDL如何求解几何雅克比矩阵

【机器人学】机器人开源项目KDL源码学习：（1）下载源码并在ubuntu下运行geometry部分（旋转矩阵）

相关推荐

ROS入门学习记录（一） 树莓派4B安装ROS+远程登录

ROS2时间同步(python)

ROS学习记录（一） Plugin插件

ROS1结合自动驾驶数据集Kitti开发教程(四)画出自己车子模型以及照相机视野

Ubuntu助手 -- 一键自动安装软件，一键进行系统配置

从零开始搭二维激光SLAM --- 基于Hector的栅格地图的构建

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

ROS入门学习记录（一）树莓派4B安装ROS+远程登录

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板