ROS机械臂，MoveIt!自学笔记（一）

1 机器人模型创建

1.1 URDF建模原理

机器人组成（控制角度）

• 控制系统

• 关节控制 - 人机交互 - 系统监督 - 算法计算 - 处理器

• 驱动系统

• 电驱动 - 液压驱动 - 气压驱动

• 执行机构

• 电机 - 伺服 - 传动机构

• 传感系统

• 里程计 - 陀螺仪 - 加速度计 - 摄像头

什么是URDF？

1. 一种使用XML格式描述的机器人模型文件。

   • Links：坐标系与几何关系

   • Joints：Links之间的连接关系

2. 类似于D-H参数

3. 可以描述完成的工作空间，不局限于机器人

link标签

1. 描述机器人某个刚体部分的外观和物理属性；
2. 描述连杆尺寸（size）、颜色（color）、形状（shape）、惯性矩阵（inertial metrix），碰撞参数（collision properties）等；
3. 每个Link会成为一个坐标系。

joint标签

1.描述两个link之间的关系，分为六种类型；

• ​ continuous 旋转关节，可以围绕单轴无限旋转
• ​ revolute 旋转关节，类似于continuous，但是有旋转的角度极限
• ​ prismatic 滑动关节，沿某一轴线移动的关节，带有位置极限
• ​ planar 平面关节，允许在平面正交方向上平移或者旋转
• ​ floating 浮动关节，允许进行平移、旋转运动
• ​ fixed 固定关节，不允许运动的特殊关节

2.包括关节运动的位置和速度限制；

3.描述机器人关节的运动学和动力学属性。

robot标签

1.完整机器人模型的最顶层标签

2.link和joint标签都必须包含robot标签内

3.一个完整的机器人模型，由一系列link和joint组成

URDF建模存在哪些问题？

• 模型冗长，重复过多
• 参数修改麻烦，不便于二次开发
• 没有参数计算的功能；
• ……

1.2 机械臂URDF建模

URDF模型的进化版本——xacro模型文件

• 精简模型代码
  • 创建宏定义
  • 文件包含
• 提供可编程接口
  • 常量
  • 变量
  • 数学计算
  • 条件语句

2 MoveIt功能包

2.1 MoveIt!简介

• 一个易于使用的集成化开发平台
• 由一系列移动操作的功能包组成
  • 运动规划
  • 操作控制
  • 3D感知
  • 运动学
  • 控制与导航算法
  • ……
• 提供友好的GUI
• 可应用于工业、商业、研发和其他领域
• ROS社区中使用度排名前三的功能包

MoveIt!三大核心功能

**运动学：**KDL、Trac-IK、IKFasft……

**路径规划：**OMPL、CHOMP、SBPL……

**碰撞检测：**FCL、PCD……

https://zhuanlan.zhihu.com/p/90784465

MoveIt!组成

• 用户接口（User Interface）
  • C++：使用move_group_interface包提供的API
  • Python：使用moveit_commander包提供的API
  • GUI：使用MoveIt!的rviz插件
• ROS参数服务器
  • URDF：robot_description参数，获取机器人URDF模型的描述信息
  • SRDF：robot_description_semantic参数，获取机器人模型的配置信息
  • config：机器人的其他配置信息，例如关节限位、运动学插件、运动规划插件等
• 机器人
  • Topic和Action通信

MoveIt!流程

• **组装：**创建机器人URDF模型
• **配置：**使用MoveIt! Setup Assistant工具生成配置文件
• **驱动：**添加机器人控制器插件（controller）
• **控制：**MoveIt!控制机器人运动（算法仿真、物理仿真）

2.2 MoveIt!可视化配置

Moveit!可视化配置_qq_46095529的博客-优快云博客_可视化配置

10分钟玩转MoveIt可视化配置和仿真（内有福利） - 古月居 (guyuehome.com)

1.move_group:move_group是moveit的核心部分，可以综合机器人的各独立组件，为用户提供一系列的动作指令和服务。move_group类似于一个积分器，本身并不具备丰富的功能，主要做各功能包和插件的集成。它通过消息或服务的形式接收机器人上传的点云信息，joints的状态消息，还有机器人的tf tree，另外还需要ros的参数服务器提供机器人的运动学参数，这些参数会在使用setup assistant的过程中根据机器人的URDF模型文件，创建生成，包括SRDF文件和配置文件。

2.motion panning（运动规划）：在moveit中，运动规划算法是由运动规划器（motion planner）完成，而规划器是作为插件来安装的，可以通过ROS的pluginlib接口来加载需要的规划器。运动规划算法有很多，每一个运动规划器都是moveit的一个插件，可以根据需求选用不同的规划算法，move_group默认使用的是OMPL算法。

3.Planning Scene（规划场景）：可以为机器人创建一个具体的工作环境，加入一些障碍物。

4.Kinematics（运动学）：运动学算法是机械臂各种算法中的核心，尤其是逆运动学算法IK（inverse kinematics）。什么是逆运动学(IK)？简单说，就是把终端位姿变成关节角度，q=IK§。p是终端位姿(xyz)，q是关节角度。为什么要用IK？OMPL是采样算法，也就是要在关节空间采样。 这与无人车的规划有一个最明显的区别，无人车的目标就是在采样空间， e.g. 目标是(x,y), 采样空间也是(x,y). 但是对于机械臂，目标是终端空间位置(xyz), 但采样空间却是关节空间(q0,q1,…qN)。有了IK之后，我们就可以把三维空间的目标p转化为关节空间的目标q。那么这样就会让采样算法能算的更快，具体方法不赘述，这样的算法有RRT-Connect，BKPIECE等等双向采样算法。moveit使用插件的形式可以让用户灵活的选择需要使用的反向运动学算法，也可以选择自己的算法。moveit中默认的IK算法是numerical jacobian-base算法。

5.collision checking（碰撞检测）：moveit使用CollisionWorld对象进行碰撞检测，采用FCL（Flexible Collision Library）功能包。碰撞检测是运动规划中最耗时的运算，往往会占用90%左右的时间，为了减少计算量，可用通过设置ACM（Allowed Collision Matrix）来进行优化，如果两个bodys之间的ACM设置为1，则意味着这两个bodys永远不会发生碰撞，不需要进行碰撞检测。

6.OMPL（open motion planning library）：开源运动规划库，是一个运动规划的C++库，其中包含了很多运动规划领域的前沿算法。虽然OMPL里面提到了最优规划，但总体来说OMPL还是一个采样规划算法库。而采样规划算法中，最出名的莫过于RRT（rapidly-exploring random trees）和PRM（probabilistic roadmap）。 OMPL能做什么？ 简单说，就是提供一个运动轨迹。给定一个机器人结构(假设有N个关节)，给定一个目标(比如终端移到xyz)，给定一个环境，那么OMPL会提供给你一个轨迹，包含M个数组，每一个数组长度是N，也就是一个完整的关节位置。沿着这个轨迹依次移动关节，就可以最终把终端移到xyz，当然，这个轨迹应当不与环境中的任何障碍发生碰撞。为什么用OMPL？ 运动规划的软件库和算法有很多，而OMPL由于其模块化的设计和稳定的更新，成为最流行的规划软件库之一。很多新算法都在OMPL开发。很多其他软件（包括ROS/MoveIt）都使用OMPL做运动规划。

7.用moveit控制机器人大概分以下几步：
a.建立机器人URDF模型
b.建立机器人ros驱动
c.生成moveit配置文件
d.标定相机
e.修改moveit配置文件和launch文件
其中在进行仿真操作过程中，不需要b，d，e三个步骤。

2.3 在仿真环境玩转机械臂

2.3.1 ROS中的控制器插件

ros_control是什么？

• ROS为开发者提供的机器人控制的中间件
• 包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等
• 可以帮助机器人应用功能包快速落地，提高开发效率

**控制器管理器：**提供一种通用的接口来管理不同的控制器

**控制器：**读取硬件状态，发布控制命令，完成每个joint的控制

**硬件资源：**为上下两层提供硬件资源的接口

**机器人硬件抽象：**机器人硬件抽象和硬件资源直接打交道，通过write和read方法完成硬件操作

**真实机器人：**执行接收到的命令

控制器（Controllers）：

• joint_state_controller：监控状态，发布topic。将注册到hardware_interface::JointStateInterface的所有资源的状态发布到类型为sensor_msgs/JointState的topic——joint_state_controller。
• joint_effort_controller：在关节上施加所需的力/扭矩
• joint_position_controller：最常用，位置控制
• joint_velocity_controller：速度输入进入PID控制器，该PID控制器向关节输出力/扭矩

2.3.2 完善机器人模型

**第一步：**为link添加惯性参数和碰撞属性

**第二步：**为joint添加传动装置 transmission，在这里添加输入，输出，驱动器，减速比等

**第三步：**添加gazebo控制器插件

在gazebo中加载机器人模型

moveit输入用户指令，比如起始位置，输出轨迹数据，每个点通过位置速度加速度描述，为了通过话题发出来，还需要一个Follow Joint Trajectory功能，通过通信接口发给机器人，机器人还需要对轨迹进行精插补，再驱动电机同步运动。机器人控制器还需要将每个电机的状态数据反馈给moveit来确定机器人是否到达指定位置，这样完成一个闭环。

2.3.3 构建Gazebo仿真

明确机械臂控制器接收哪些数据？

**Joint Trajectory Controller：**probot_gazebo/config/probot_anno_trajectory_control.yaml

**Joint State Controller：**probot_gazebo/config/probot_anno_gazebo_joint_states.yaml

**Follow Joint Trajectory：**moveit端控制器接口

关节轨迹控制器

• 线性样条：位置连续，速度、加速度不连续
• 三次样条：位置和速度连续，加速度不连续
• 五次样条：位置、速度、加速度都连续

控制器启动

probot_gazebo/…/probot_anno_trajectory_controller.launch

**打印轨迹：**rostopic echo /probot_anno/arm_joint_controller/follow_joint_trajectory/goal

里面包括关节的一些信息等

2.4 MoveIt!编程，驾驭机械臂运动控制

2.4.1 两种编程接口

“move _group" Python Interface

group = moveit_commander.MoveGroupCommander("left_arm")

pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)

plan1 = group.plan()

“move_group” C++ Interface

moveit::planning_interface::MoveGroup group("right_arm");

geometry_msgs::Pose target_pose;
target_pose.orientation.w = 1.0;
target_pose.position.x = 0.28;
target_pose.position.y = -0.7;
target_pose.position.z = 1.0;
group.setPoseTarget(target_pose);

moveit::planning_interface::MoveGroup::Plan my plan;
bool success = group.plan(my_plan);

编程流程

• 连接控制需要的规划组（控制谁）
• 设置目标位姿（关节空间或笛卡尔空间）
• 设置运动约束（可选，姿态等，比如端水不想打翻）
• 使用MoveIt!规划一条到达目标的轨迹
• 修改轨迹（如速度等参数）
• 执行规划出的轨迹