[gazebo仿真1]Kinova机械臂gazebo控制

gazebo加载完kinova机械臂后，通过joint_state_publisher_gui节点控制机械臂在gazebo中的姿态

1.编写转换节点
joint_state_publisher_gui节点发布的topic是/joint_states消息类型为 sensor_msgs/JointState
而控制gazebo中机械臂进行运动的topic为/’ + 机械臂型号 + '/effort_joint_trajectory_controller/command消息类型为 trajectory_msgs/JointTrajectory
需要进行一个简单的转换
在kinova官方包给的move_robot.py文件的基础上进行改写，如下：

#! /usr/bin/env python
"""Publishes joint trajectory to move robot to given pose"""

import rospy
from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from std_srvs.srv import Empty
from sensor_msgs.msg import JointState
import argparse
import time


def moveJoint (jointcmds,prefix,nbJoints):
  topic_name = '/' + prefix + '/effort_joint_trajectory_controller/command'
  pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
  jointCmd = JointTrajectory()  
  point = JointTrajectoryPoint()
  jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0);  
  point.time_from_start = rospy.Duration.from_sec(5.0)
  for i in range(0, nbJoints):
    jointCmd.joint_names.append(prefix +'_joint_'+str(i+1))
    point.positions.append(jointcmds[i])
    point.velocities.append(0)
    point.accelerations.append(0)
    point.effort.append(0) 
  jointCmd.points.append(point)
  rate = rospy.Rate(100)
  count = 0
  # print(jointCmd)
  # while (count < 50):
  #   pub.publish(jointCmd)
  #   count = count + 1
  #   rate.sleep() 
  pub.publish(jointCmd)
  rate.sleep()    

def moveFingers (jointcmds,prefix,nbJoints):
  topic_name = '/' + prefix + '/effort_finger_trajectory_controller/command'
  pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)  
  jointCmd = JointTrajectory()  
  point = JointTrajectoryPoint()
  jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0);  
  point.time_from_start = rospy.Duration.from_sec(5.0)
  for i in range(0, nbJoints):
    jointCmd.joint_names.append(prefix +'_joint_finger_'+str(i+1))
    point.positions.append(jointcmds[i])
    point.velocities.append(0)
    point.accelerations.append(0)
    point.effort.append(0)

  jointCmd.points.append(point)
  rate = rospy.Rate(100)
  count = 0
  # while (count < 50):
  #   pub.publish(jointCmd)
  #   count = count + 1
  #   rate.sleep()     

  pub.publish(jointCmd)
  rate.sleep() 

def moveFingersTip(pose,prefix,tip):
  topic_name = '/' + prefix + '/finger_tip_' + tip + '_position_controller/command'
  pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
  jointCmd = JointTrajectory()  
  point = JointTrajectoryPoint()
  jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0);  
  point.time_from_start = rospy.Duration.from_sec(5.0)

  jointCmd.joint_names.append(prefix +'_joint_finger_tip_'+tip)
  point.positions.append(pose)
  point.velocities.append(0)
  point.accelerations.append(0)
  point.effort.append(0)
  jointCmd.points.append(point)
   
  # print("fingertip--------------------------------------")
  # print(jointCmd)
  pub.publish(jointCmd)
  

def call_back(data):
  # function
  # print("******this is call_back function********") 
  # print(data)
  # move joint
  moveJoint([data.position[0],data.position[1],data.position[2],data.position[3],data.position[4],data.position[5]],"j2n6s300",6) 
  # move finger
  moveFingers ([data.position[6],data.position[8],data.position[10],data.position[7],data.position[9],data.position[11]],"j2n6s300",3)
  # move figer tip have some problems
  moveFingersTip(100,"j2n6s300","1")
  moveFingersTip(data.position[9],"j2n6s300","2")
  moveFingersTip(data.position[11],"j2n6s300","3")

if __name__ == '__main__':
  try:    
    rospy.init_node('gui2gazebo')		
    time.sleep(5)

    # Unpause the physics
    rospy.wait_for_service('/gazebo/unpause_physics')
    unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
    resp = unpause_gazebo()

    #create a subscriber to get the gui info
    rospy.Subscriber("/joint_states",JointState,call_back)
    rospy.spin()

  except rospy.ROSInterruptException:
    print "program interrupted before completion"

2.效果演示：

roslaunch kinova_gazebo robot_launch.launch      # 启动仿真环境


rosrun kinova_control translate_node_kinova.py   #启动转换节点   

rosrun joint_state_publisher_gui joint_state_publisher_gui   #启动gui

默认的机械臂型号为j2n6s300

指尖还不能控制有报错，消息类型不一致，修改一下

把指尖的函数改为下面

def moveFingersTip(pose,prefix,tip):
  topic_name = '/' + prefix + '/finger_tip_' + tip + '_position_controller/command'
  # pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
  pub = rospy.Publisher(topic_name, Float64, queue_size=1)
  pub.publish(pose)

依旧有警告不过，指尖已经可以控制了