Go to the documentation of this file.00001 import xmltodict
00002 from collections import namedtuple
00003 import numpy as np
00004 import rospy
00005 import roslib
00006
00007 Pose = namedtuple('Pose', ['xyz', 'rpy'], verbose=True)
00008
00009 def get_pose_array(filter_array, palm, rpy=[0,0,0]):
00010 pa = []
00011
00012 for i in range(len(filter_array)):
00013 loc_string = filter_array[i]['@params'].split(',')
00014 loc = np.array(loc_string, dtype=float)
00015
00016 xyz = np.mean(loc.reshape((2,3)),0)/1000
00017 if palm:
00018
00019
00020 xyz = [xyz[0], xyz[1], 0.075]
00021 else:
00022 xyz = [xyz[1], xyz[0], xyz[2]]
00023
00024 pa.append(Pose(xyz=xyz, rpy=rpy))
00025
00026 return pa
00027
00028 def get_path(package_name, resource_name):
00029 resources = roslib.packages.find_resource(package_name, resource_name)
00030 if len(resources) == 0:
00031 rospy.logerr("Failed to find resource %s in package %s"%(resource_name, package_name))
00032 return ""
00033 else:
00034 return resources[0]
00035
00036 tactile_filename = get_path('graspit', 'BarrettBH8_280_Tactile.xml')
00037
00038 with open(tactile_filename) as fd:
00039 doc = xmltodict.parse(fd.read())
00040
00041 palm_sensors = get_pose_array(doc['robot']['filter'], True)
00042
00043 barrett_sensors = {}
00044 barrett_sensors['palm'] = palm_sensors
00045
00046 for index, chain_num in enumerate(range(len(doc['robot']['chain'])), 1):
00047
00048 finger_sensors = get_pose_array(doc['robot']['chain'][chain_num]['filter'], False)
00049 barrett_sensors['link{}'.format(index)] = finger_sensors
00050
00051
00052 text_poses = ""
00053 for i, pose in enumerate(barrett_sensors['palm'], 1):
00054 text_poses += '<joint name="${{name}}_palm_sensor{}_joint" type="fixed">\n'.format(i)
00055 text_poses += '\t<origin xyz="{} {} {}" rpy="{} {} {}"/>\n'.format(pose.xyz[0], pose.xyz[1], pose.xyz[2], pose.rpy[0], pose.rpy[1], pose.rpy[2])
00056 text_poses += '\t<parent link="${name}_base_link"/>\n'
00057 text_poses += '\t<child link="${{name}}_palm_sensor_{}_link"/>\n'.format(i)
00058 text_poses += '</joint>\n'
00059 text_poses += '<link name="${{name}}_palm_sensor_{}_link"/>\n'.format(i)
00060
00061 for link_num in range(1, 4):
00062 for i, pose in enumerate(barrett_sensors['link{}'.format(link_num)], 1):
00063 text_poses += '<joint name="${{name}}_link{}_sensor{}_joint" type="fixed">\n'.format(link_num, i)
00064 text_poses += '\t<origin xyz="{} {} {}" rpy="{} {} {}"/>\n'.format(pose.xyz[0], pose.xyz[1], pose.xyz[2], pose.rpy[0], pose.rpy[1], pose.rpy[2])
00065 text_poses += '\t<parent link="bh_finger_{}4_link"/>\n'.format(link_num)
00066 text_poses += '\t<child link="${{name}}_link{}_sensor{}_link"/>\n'.format(link_num, i)
00067 text_poses += '</joint>\n'
00068 text_poses += '<link name="${{name}}_link{}_sensor{}_link"/>\n'.format(link_num, i)
00069
00070
00071 import pyperclip
00072 pyperclip.copy(text_poses)