文章目录
在最新的Moveit中提供了moveit_jog_arm包,可以实现机械臂的点动效果。现在就结合moveit提供的教材分析一下其工作的机制
![](https://i-blog.csdnimg.cn/blog_migrate/b807044ee8883293f7da5cca25a25692.png)
启动文件的分析
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.0
y: 0.01
z: -0.01
angular:
x: 0.0
y: 0.0
z: 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**获取的当前关节位置,便得到了新的关节位置。