MoveIt的使用（三）机械臂在rviz中的可视化以及编程控制

在第一篇文章中，我们实现了从solidworks到urdf文件的转换，第二篇文章，主要介绍了MoveIt！Setup Assistant的配置，以及机械臂的初步可视化，下面我们继续实现机械臂在rviz中的可视化以及编程控制。

一、机械臂的rviz可视化

打开配置好moveit的机械臂文件

source devel/setup.bash
roslaunch marm_moveit_config demo.launch

可以鼠标移动机械臂末端的小球，机械臂的各个关节也会进行转动使机械臂的末端始终在小球处。

1.可视化

2.各关节角度控制

3.选择随机位置，进行规划

二、初步编程控制

在生成的配置文件和三维模型代码所在的工作空间，创建功能包。
建立moveit_fk_demo.py文件，内容如下，本次使用的机械臂没有设置末端执行器，所以代码中关于末端执行器的代码已经注释掉，如有需要再解除注释。

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand

class MoveItFkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_fk_demo', anonymous=True)
 
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')
        
        # 初始化需要使用move group控制的机械臂中的gripper group
        #gripper = moveit_commander.MoveGroupCommander('gripper')
        
        # 设置机械臂和夹爪的允许误差值
        arm.set_goal_joint_tolerance(0.001)
        #gripper.set_goal_joint_tolerance(0.001)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
         
        # 设置夹爪的目标位置，并控制夹爪运动
        #gripper.set_joint_value_target([0.01])
        #gripper.go()
        #rospy.sleep(1)
         
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [-1.4311, 0.4188, 0.5759, -0.4886, 0.4712]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItFkDemo()
    except rospy.ROSInterruptException:
        pass

编译好后，启动rviz，然后运行上述功能包。

roslaunch marm_moveit_config demo.launch
rosrun marm_planning moveit_fk_demo.py

上述代码，先让机械臂回到预先定义的home状态，然后再自动规划到下面代码设置到位置。

joint_positions = [-1.4311, 0.4188, 0.5759, -0.4886, 0.4712]

代码中这一行是，从关节joint1到joint5，各个关节的弧度，可以调节这几个数字，让机械臂呈现不同的姿态。