威控复合机器人(二)------机械臂的控制

---

一、启动机械臂的控制

在使用机械臂的时候我们先要启动相关的launch文件

启动机械臂控制

roslaunch vkarm_controller vkarm_controller.launch

启动gui机械臂控制

roslaunch vkarm_control_gui vkarm_control_gui.launch

启动机械臂服务

roslaunch vkarm_controller vkarm_control_bsp.launch

二、写机械臂的执行代码(python)

1.加载相关的初始化模块

#!/usr/bin/env python  
#coding=utf8
#加载需要的模块库
import rospy 
#加载机械臂需要的模块文件
from vkarm_msgs.srv import PythontoSetJointPosition

#机械臂init姿态
init=[0.0,0.0,0.0,0.0,2]
#机械臂home姿态
home=[0.0,-0.7,0.0,0.7,2]

2.初始化相关节点，并订阅机械臂的控制话题

    def __init__(self):  
        rospy.init_node('vkbot_competition', anonymous=False)  
        rospy.on_shutdown(self.shutdown)  
        rospy.wait_for_service('/vkarm/python_joint')
        rospy.loginfo("Connected to vkarm_control server!")
    def shutdown(self):  
        rospy.loginfo("Stopping the vkbot...")  
        rospy.sleep(2)

3.初始化机械臂的关节值

    def vkarm_set_joint(self,data):  
        joint = PythontoSetJointPosition()
        joint.joint_1 = data[0]
        joint.joint_2 = data[1]
        joint.joint_3 = data[2]
        joint.joint_4 = data[3]
        joint.time    = data[4]

4.调用机械臂的客户端，并对机械臂的关节进行操作

        try:
           vkarm_python_joint = rospy.ServiceProxy('/vkarm/python_joint', PythontoSetJointPosition)
           response = vkarm_python_joint(joint.joint_1,joint.joint_2,joint.joint_3,joint.joint_4,joint.time)
           rospy.loginfo("vkarm joint control ok!!")
           return response.is_ok
        except rospy.ServiceException, e:
           print "Service call failed: %s"%e

5. 主函数

if __name__ == '__main__':  
    try:  
        # 初始化类
        vkbot_tast=vkbot()
        rospy.sleep(2)
	    vkbot_tast.vkarm_set_joint(init)
        rospy.sleep(2)
	    vkbot_tast.vkarm_set_joint(home)
        rospy.sleep(2)
    except rospy.ROSInterruptException:
        rospy.loginfo("vkbot mission two finished.")

6. 总代码

#!/usr/bin/env python  
#coding=utf8
#加载需要的模块库
import rospy 
#加载机械臂需要的模块文件
from vkarm_msgs.srv import PythontoSetJointPosition

#机械臂init姿态
init=[0.0,0.0,0.0,0.0,2]
#机械臂home姿态
home=[0.0,-0.7,0.0,0.7,2]

class vkbot():
    def __init__(self):
        rospy.init_node('vkbot_competition', anonymous=False)  
        rospy.on_shutdown(self.shutdown)
        rospy.wait_for_service('/vkarm/python_joint')
        rospy.loginfo("Connected to vkarm_control server!")
    def shutdown(self):  
        rospy.loginfo("Stopping the vkbot...")  
        rospy.sleep(2)

    def vkarm_set_joint(self,data):  
        joint = PythontoSetJointPosition()
        joint.joint_1 = data[0]
        joint.joint_2 = data[1]
        joint.joint_3 = data[2]
        joint.joint_4 = data[3]
        joint.time    = data[4]
        try:
           vkarm_python_joint = rospy.ServiceProxy('/vkarm/python_joint', PythontoSetJointPosition)
           response = vkarm_python_joint(joint.joint_1,joint.joint_2,joint.joint_3,joint.joint_4,joint.time)
           rospy.loginfo("vkarm joint control ok!!")
           return response.is_ok
        except rospy.ServiceException, e:
           print "Service call failed: %s"%e

if __name__ == '__main__':  
    try:
        vkbot_tast=vkbot()
        rospy.sleep(2)
	    vkbot_tast.vkarm_set_joint(init)
        rospy.sleep(2)
	    vkbot_tast.vkarm_set_joint(home)
        rospy.sleep(2)
    except rospy.ROSInterruptException:
        rospy.loginfo("vkbot mission two finished.")