python跟踪UR5机械臂示教的轨迹(机械臂末端点坐标),并保存成txt/json格式

  • Post author:
  • Post category:python




1.描述:

用python写了一段代码,用来保存通过示教操作的机械臂运行轨迹。这份代码在之前的ubuntu16.04上运行正常,但是切换到ubuntu18.04的电脑上,就会提示报错,(例如提示taberror,说代码空格的形式不对,print x,y,z没有带括号等,详见第3节debug过程),最后才找到核心的问题,在ubuntu18.04的系统上,要屏蔽掉roslib.load_manifest(‘learning_tf’)这个代码,具体的原因我对ROS系统不了解所以不是特别理解。最后这段代码成功运行并且保存了机械臂的运行轨迹。



2.先贴上原封不动的代码(没屏蔽之前,报错的那份)

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
import sys	
import time
import json
import string


def text_save(filename, data):#.#filename为写入txt文件的路径,data为要写入数据列表.
    file = open(filename,'a')   
    for i in range(len(data)):
        s = str(data[i]).replace('[','').replace(']','')#
        s = s.replace("'",'').replace(',','') +' '   #
        file.write(s)
    enter = '\n'
    file.write(enter)
    file.close()
if __name__ == '__main__':
       rospy.init_node('turtle_tf_listener')
       
       
       
      
       listener = tf.TransformListener()

       
       #rospy.wait_for_service('spawn')
       #spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
       #spawner(4, 2, 0, 'turtle2')
   
       #turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
   	
       #rate = rospy.Rate(10.0)#HZ
       while not rospy.is_shutdown():
           try:
              (trans,rot) = listener.lookupTransform('/base_link', '/ee_link', rospy.Time(0))

           except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
               continue

           #angular = 4 * math.atan2(trans[1], trans[0])
           #linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
           #cmd = geometry_msgs.msg.Twist()
           #cmd.linear.x = linear
           #cmd.angular.z = angular
           #turtle_vel.publish(cmd)
	   x=trans[0]
	   y=trans[1]
	   z=trans[2]
	   ox = rot[0]
	   oy = rot[1]
	   oz = rot[2]
	   ow = rot[3]
	   text_save('2.txt',trans)
	  
	   print x,y,z
	   jsonArr = json.dumps((x,y,z),ensure_ascii=False)
	   f = open("a.json","a")
	   f.write(jsonArr)
           f.close()
   	   #print((trans,rot))
	   print("\n")
           time.sleep(0.01)






在这个python程序运行之前,还需要用rostopic把数据发布出来,依次运行代码:

(base) desmond@desmond-throatswap:~$ roscore
(base) desmond@desmond-throatswap:~$ roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=192.168.1.100
(base) desmond@desmond-throatswap:~$ roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch
(base) desmond@desmond-throatswap:~$ rostopic echo /joint_states

直到命令行打印如下的数据(x,y,z坐标和四元数)即OK,这四行指令也可以用脚本(.sh文件)或者roslaunch文件运行



注意:



第2,3句指令格式roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=192.168.1.100和roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch需要修改,修改的方法:搜索ur5_bringup.launch所在的package,然后把ur_robot_driver改为对应的package即可,两个指令都是这么改

robot_ip需要修改成机械臂对应的ip,如果不知道可以在示教器的上面查询,UR5设置(setup)-网络(network)那里可以看IP地址,在修改这个robot_ip前可以ping一下看看通信是否正常(例如:ping 192.168.1.100),正常的话会一直返回数据

position: [1.5888543128967285, -0.8793647925006312, 0.11490737646818161, -0.6185405890094202, 1.7124810218811035, 3.1320056915283203]

velocity: [0.05301181972026825, 0.10915552079677582, 0.008104532025754452, 0.04115382954478264, 0.0, 0.0]



3. Debug过程

3.1 报错:

(base) desmond@desmond-throatswap:~/lee/ThroatSwap/WQW$ python turtle_tf_listenerOK.py
  File "turtle_tf_listenerOK.py", line 53
    x=trans[0]
             ^
TabError: inconsistent use of tabs and spaces in indentation

3.2 按照格式修改完空格以后,又出现了新的报错,很奇怪如果代码格式不对,怎么在ubuntu16.04的工控机上运行一切正常

desmond@desmond-throatswap:~/lee/ThroatSwap/WQW$ python3 turtle_tf_listener.py
  File "turtle_tf_listener.py", line 62
    print x,y,z
          ^
SyntaxError: Missing parentheses in call to 'print'. Did you mean print(x,y,z)?

3.3 打印xyz(机械臂末端店位置)改成print(x,y,z)以后

..............
	   text_save('2.txt',trans)
	  
	   print (x,y,z)
	   jsonArr = json.dumps((x,y,z),ensure_ascii=False)
	   f = open("a.json","a")
	   ........................................

又出现新的报错:

(base) desmond@desmond-throatswap:~/lee/ThroatSwap/WQW$ python turtle_tf_listener.py
Traceback (most recent call last):
  File "turtle_tf_listener.py", line 3, in <module>
    roslib.load_manifest('learning_tf')
  File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 64, in load_manifest
    sys.path = _generate_python_path(package_name, _rospack) + sys.path
  File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 97, in _generate_python_path
    m = rospack.get_manifest(pkg)
  File "/home/desmond/miniconda3/lib/python3.8/site-packages/rospkg/rospack.py", line 171, in get_manifest
    return self._load_manifest(name)
  File "/home/desmond/miniconda3/lib/python3.8/site-packages/rospkg/rospack.py", line 215, in _load_manifest
    retval = self._manifests[name] = parse_manifest_file(self.get_path(name), self._manifest_name, rospack=self)
  File "/home/desmond/miniconda3/lib/python3.8/site-packages/rospkg/rospack.py", line 207, in get_path
    raise ResourceNotFound(name, ros_paths=self._ros_paths)
rospkg.common.ResourceNotFound: learning_tf
ROS path [0]=/opt/ros/melodic/share/ros
ROS path [1]=/opt/ros/melodic/share

3.4 检索这个报错rospkg.common.ResourceNotFound: learning_tf,网上找了一圈方法,包括source setup.bash, apt-get install learning_tf都没用

3.5 仔细看代码,发现代码头文件那里有一个learning_tf,网上查了一下roslib.load_manifest(‘learning_tf’)这个指令的意思,解答如下


what does roslib.load_manifest() do?


daima在这里插入图片描述

根据上面那个网页链接的描述,我的理解是在catkin的版本不再需要roslib.load_manifest()了,于是把这个屏蔽掉,3.3这个rospkg.common.ResourceNotFound: learning_tf 报错解决了,此时的代码格式如下(已经把空格的问题和print的问题改完):

#!/usr/bin/env python  
import roslib
#roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
import sys 
import time
import json
import string


def text_save(filename, data):#.
    file = open(filename,'a')
    for i in range(len(data)):
        s = str(data[i]).replace('[','').replace(']','')#
        s = s.replace("'",'').replace(',','') +' '   #
        file.write(s)
    enter = '\n'
    file.write(enter)
    file.close()
if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')
       
       
       
      
    listener = tf.TransformListener()

       
    #rospy.wait_for_service('spawn')
    #spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    #spawner(4, 2, 0, 'turtle2')
   
    #turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
       
    #rate = rospy.Rate(10.0)#HZ
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/ee_link', '/base_link', rospy.Time(0))

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        #angular = 4 * math.atan2(trans[1], trans[0])
        #linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        #cmd = geometry_msgs.msg.Twist()
        #cmd.linear.x = linear
        #cmd.angular.z = angular
        #turtle_vel.publish(cmd)
    x=trans[0]
    y=trans[1]
    z=trans[2]
    ox = rot[0]
    oy = rot[1]
    oz = rot[2]
    ow = rot[3]
    text_save('2.txt',trans)
     
    print (x,y,z)
    jsonArr = json.dumps((x,y,z),ensure_ascii=False)
    f = open("a.json","a")
    f.write(jsonArr)
    f.close()
    #print((trans,rot))
    print("\n")
    time.sleep(0.01)




但是又出现了新的报错:

desmond@desmond-throatswap:~/lee/ThroatSwap/WQW$ python3 turtle_tf_listener.py
Traceback (most recent call last):
  File "turtle_tf_listener.py", line 6, in <module>
    import tf
  File "/opt/ros/melodic/lib/python2.7/dist-packages/tf/__init__.py", line 30, in <module>
    from tf2_ros import TransformException as Exception, ConnectivityException, LookupException, ExtrapolationException
  File "/opt/ros/melodic/lib/python2.7/dist-packages/tf2_ros/__init__.py", line 38, in <module>
    from tf2_py import *
  File "/opt/ros/melodic/lib/python2.7/dist-packages/tf2_py/__init__.py", line 38, in <module>
    from ._tf2 import *
ImportError: dynamic module does not define module export function (PyInit__tf2)

报错当中出现了这个报错的意思是因为tf2_ros是为python2编译的,但我在命令行中运行的是python3 turtle_tf_listener.py(如果运行的是python turtle_tf_listener.py,要用python –version查看一下当前的python版本),所以网上找到了一个方法解决这个问题


ImportError: dynamic module does not define module export function (PyInit__tf2)


不过我没有用这个方法,我直接把运行命令改为python2 turtle_tf_listener.py,发现可以保存一行指令,但是没有打印x,y,z坐标(命令print ,x,y,z 的功能),卡在那里没有动

3.6 真相:

用第2节里面的代码,不改格式(不改空格,print这些),直接屏蔽掉roslib.load_manifest(‘learning_tf’)这一句,然后保存运行(python2 turtle_tf_listener.py ),就OK了

#!/usr/bin/env python  
import roslib
#roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
import sys 
import time
import json
import string


def text_save(filename, data):#.
    file = open(filename,'a')
    for i in range(len(data)):
        s = str(data[i]).replace('[','').replace(']','')#
        s = s.replace("'",'').replace(',','') +' '   #
        file.write(s)
    enter = '\n'
    file.write(enter)
    file.close()
if __name__ == '__main__':
       rospy.init_node('turtle_tf_listener')
       
       
       
      
       listener = tf.TransformListener()

       
       #rospy.wait_for_service('spawn')
       #spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
       #spawner(4, 2, 0, 'turtle2')
   
       #turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
       
       #rate = rospy.Rate(10.0)#HZ
       while not rospy.is_shutdown():
           try:
              (trans,rot) = listener.lookupTransform('/ee_link', '/base_link', rospy.Time(0))

           except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
               continue

           #angular = 4 * math.atan2(trans[1], trans[0])
           #linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
           #cmd = geometry_msgs.msg.Twist()
           #cmd.linear.x = linear
           #cmd.angular.z = angular
           #turtle_vel.publish(cmd)
      x=trans[0]
      y=trans[1]
      z=trans[2]
      ox = rot[0]
      oy = rot[1]
      oz = rot[2]
      ow = rot[3]
      text_save('2.txt',trans)
     
      print x,y,z
      jsonArr = json.dumps((x,y,z),ensure_ascii=False)
      f = open("a.json","a")
      f.write(jsonArr)
           f.close()
          #print((trans,rot))
      print("\n")
           time.sleep(0.01)






4. 总结

代码本身的格式没有问题,print x,y,z是正确的,只需要屏蔽掉roslib.load_manifest(‘learning_tf’)这一句即可运行



版权声明:本文为weixin_42314494原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。