ROS&Moveit中机械臂的点动(Jog)实现

在最新的Moveit中提供了moveit_jog_arm包，可以实现机械臂的点动效果。现在就结合moveit提供的教材分析一下其工作的机制

启动文件的分析

ur_gazebo/ur5.launch

1. 打开一个空的gazebo环境
2. 加载机器人的urdf到参数服务器，robot_description
3. 通过gazebo_ros包将robot_description的机器人模型加载到gazebo。同时gazebo_ros继承RobotHW类，通过JointStateInterface提供关节状态信息，通过EffortJointInterface、PositionJointInterface以及VelocityJointInterface提供力/力矩、位置和速度的控制。
4. 启动robot_state_publisher节点，发布/tf主题
5. 通过controller_manager启动joint_state_controller，从gazebo_ros获取关节状态信息，并发布到"/joint_states"主题。
6. 通过controller_manager加载arm_controller,其类型为position_controllers/JointTrajectoryController，JointTrajectoryController提供了两个轨迹命令的接收接口：action接口（启动一个action server,提供一组"arm_controller/follow_joint_trajectory/XXXX"的主题）和topic接口（单个command主题）。
7. 通过controller_manager加载joint_group_position_controller，其类型为positon_controllers/JointGroupPositionController。它接受关节位置命令。

ur5_moveit_config/ur5_moveit_planning_execution.launch

1. 将"/follow_joint_trajectory"重映射为"/arm_controller/follow_joint_trajectory"。（在没执行ur5_moveit_planning_execution.launch之前就已经是/arm_controller/follow_joint_trajectory了，不知道为什么重映射）
2. 加载move_group所需的上下文信息。
3. 启动move_group节点，在move_group中根据选择加载simple_controller_manager还是fake_controller_manager。如果是simple_controller_manager（例子中就是），则会连接到ROS的controller_manager提供的"arm_controller/follow_joint_trajectory" action接口，由moveit输出轨迹数据，**gazebo_ros端收到数据便会执行，同时通过"joint_states"主题随时反映关节的状态变化。

ur5_moveit_config/moveit_rviz.launch

1. 运行rviz，与moveit通信进行操控。

控制

控制器的切换

前面可以看到，我们加载了两个ROS控制器：joint_group_position_controller和arm_controller。arm_controller接受一组关节轨迹，这是与Moveit生成的运动轨迹相匹配的。joint_group_position_controller则是接收单个的关节位置命令，正是电动所需要的控制器。因此我们使用ROS的controller_manager提供的Service进行控制器的切换：

rosservice call /controller_manager/switch_controller "start_controllers:
- 'joint_group_position_controller'
stop_controllers:
- 'arm_controller'
strictness: 2"

可以在键入"rosservice call /controller_manager/switch_controller"后按tab健，会自动生成待填写格式，然后补全即可。

moveit_jog_arm/spacenav_cpp.launch

1. 启动spacenav_node节点，连接spacenav操控设备。
2. 启动spacenav_to_twist节点，将spacenav设备的操控指令转换为twist格式数据，twist由点的线速度和角速度向量组成。
3. 启动jog_server节点，其订阅 /jog_server/delta_jog_cmds 作为twist命令输入，订阅/joint_states"为机器人实时状态参考，将关节位置命令发布到 /joint_group_position_controller/command主题，该主题即为前面讲过的ROS控制器的命令接收端。

手动输入

在没有spacenav设备的情况下，可以手动地输入命令到/joint_group_position_controller/command，例如：

rostopic pub -r 100 /jog_server/delta_jog_cmds geometry_msgs/TwistStamped "header: auto
twist:
  linear:
    x: 0.0
    y: 0.01
    z: -0.01
  angular:
    x: 0.0
    y: 0.0
    z: 0.0"

同样可以使用tab健进行命令补全。其中"-r"参数指定发布的命令消息的赫兹频率。通过这个命令便可以使机械臂末端按指定的速度矢量运动

jog_server的工作原理

jog_server原理上其实很简单，也就是将笛卡尔空间中的位移增量转化为关节的增量。怎么转换？答案就是雅可比矩阵，我们知道关于雅可比矩阵有：
$$\frac{d\theta }{dt}=\mathbf{J}\mathcal{V}$$
其中$\mathcal{V}=[{\mathbf{v}}^T,{\omega }^T{]}^T$。因此，当我们考虑微小的时间以及速度变化量时，便有
$$\Delta \theta \approx \mathbf{J}\left[\begin{array}{c}\Delta \mathbf{v}\\ \Delta \omega \end{array}\right]$$

在jog_server的实现中，便会将输出命令周期乘以输入速度命令得到位移增量，通过雅可比变成关节增量，再加上从**/joint_states**获取的当前关节位置，便得到了新的关节位置。