moveit中RM65-B适配拓展轴一体规划

Moveit适配拓展轴

1.概述

睿尔曼关节模组和机械臂连接时可以被自动识别，并且睿尔曼机械臂提供同时控制机械臂和拓展关节模组的通信协议（限一个拓展关节）。因此，用户可以在RM机械臂的基础上添加外部关节，构建新的机器人结构并控制，方便客户针对自己的需求设计与拓展机器人以完成指定功能。

在连接外部拓展关节后，我们才仅仅可以通过机械臂json通信协议整体控制机械臂和拓展关节，运动轨迹需要我们自己设置与下发，对于场景应用不友好。

ROS中的moveit插件提供了轨迹规划功能，能够自动计算机械臂运动轨迹并下发，是场景开发的常用功能插件。

RM机械臂提供ROS驱动，能够接收moveit的运动规划。工作空间中的功能包含义如下：

序号	名称	作用
1	rm_description	机器人描述功能包，其中有创建好的机器人模型和配置文件
2	rm_moveit_config	使用Setup Assistant工具根据机械臂URDF模型rm_65.urdf.xacro创建生成的一个MoveIt配置的功能包，它包含了大部分MoveIt启动所需的配置文件和启动文件，以及包含一个简单的演示demo
3	rm_gazebo	gazebo仿真机器人所用到参数和文件配置
4	rm_demo	MoveIt编程示例，包括场景规划、避障规划和pick and place
5	rm_msgs	RM机械臂所用到的所有控制消息和状态消息
6	rm_control	机器人控制器，将Moveit规划的机械臂轨迹，通过三次样条插值细分，按照20ms的控制周期发给rm_driver节点，周期可调，但是应大于10ms
7	rm_driver	（1）与机械臂通过以太网口建立socket连接，机器人IP地址：192.168.1.18，请保证上位机的IP在同一局域网内，使用ROS控制机械臂，请务必确认机械臂处于以太网口通信模式；（2）订阅各topic数据，更新RVIZ内机械臂各关节角度
8	rm_bringup	启动机器人，运行对应的launch文件后，可自动运行rm_driver，rm_control和moveit交互RVIZ界面，直接在仿真界面中拖拽机器人即可控制真实机器人运动

但拓展关节模组在RM机械臂的ROS驱动包中并未定义。因此，若想通过Moveit一体控制RM机械臂，需要根据机器人修改对应的文件。所以整体思路应该如下：

2.适配操作

2.1确定对应话题及消息格式

首先在rm_driver下查找到对应的下发话题，可以看到moveit的规划轨迹通过jointPos_callback下发指定json消息给机械臂，所以这里的数据是最终到机械臂控制器的，我们需要将moveit规划的轨迹以正确的格式传进来。注意，官网源码中，拓展轴数据定义是expand = msg.expand，而我们需要将拓展轴和机械臂关节同时规划，保持数据一致，应该改为expand = msg.expand * RAD_DEGREE;

在rm_msgs下查找到joint_pos的消息类型，可以看到拓展轴消息expand构造，和关节一样都是float类型，因此后续发布joint_pos的消息时，如果带上拓展轴的数据，则会将拓展轴一起控制。

2.2制作urdf模型

需要根据自己的设计，给出对应的urdf模型，例如在rm_scr单臂复合机器人中，我们定义腰部旋转关节joint_arm_rotary是我们的拓展关节，因此在urdf模型中给出相应的物理关系，之后再将urdf模型转为xacro模型。这一步是后续拓展关节能够和机械臂一起运动的关键所在，只有模型关系正确，真实环境下的机器人才可以执行对应的规划轨迹。