ROSMoveit中机械臂的点动(Jog)实现

文章目录

启动文件的分析
- ur_gazebo/ur5.launch
- ur5_moveit_config/ur5_moveit_planning_execution.launch
- ur5_moveit_config/moveit_rviz.launch
控制
- 控制器的切换
- moveit_jog_arm/spacenav_cpp.launch
- 手动输入
- jog_server的工作原理

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

启动文件的分析

ur_gazebo/ur5.launch

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

ur5_moveit_config/ur5_moveit_planning_execution.launch

将"/follow_joint_trajectory"重映射为"/arm_controller/follow_joint_trajectory"。（在没执行ur5_moveit_planning_execution.launch之前就已经是/arm_controller/follow_joint_trajectory了，不知道为什么重映射）
加载move_group所需的上下文信息。
启动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

运行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

启动spacenav_node节点，连接spacenav操控设备。
启动spacenav_to_twist节点，将spacenav设备的操控指令转换为twist格式数据，twist由点的线速度和角速度向量组成。
启动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.0y: 0.01z: -0.01angular:x: 0.0y: 0.0z: 0.0"

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

jog_server的工作原理

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

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