00001 from urdf_parser_py.xml_reflection.basics import *
00002 import urdf_parser_py.xml_reflection as xmlr
00003
00004 xmlr.start_namespace('srdf')
00005
00006 verbose = True
00007
00008
00009 name_attribute = xmlr.Attribute('name', str)
00010
00011 class Link(xmlr.Object):
00012 def __init__(self, name = None):
00013 self.name = name
00014
00015 xmlr.reflect(Link, params = [
00016 name_attribute
00017 ])
00018
00019 class Joint(xmlr.Object):
00020 def __init__(self, name = None):
00021 self.name = name
00022
00023 xmlr.reflect(Joint, params = [
00024 name_attribute
00025 ])
00026
00027 class JointVal(xmlr.Object):
00028 def __init__(self, name = None, value = []):
00029 self.name = name
00030 self.value = value
00031
00032 xmlr.reflect(JointVal, params = [
00033 name_attribute,
00034 xmlr.Attribute('value', "vector")
00035 ])
00036
00037 class Sphere(xmlr.Object):
00038 def __init__(self, center = None, radius = 0.0):
00039 self.center = center
00040 self.radius = radius
00041
00042 xmlr.reflect(Sphere, params = [
00043 xmlr.Attribute('center', str),
00044 xmlr.Attribute('radius', float)
00045 ])
00046
00047
00048 link_element = xmlr.Element('link', Link, False)
00049
00050 class VirtualJoint(xmlr.Object):
00051 TYPES = ['unknown', 'fixed', 'floating', 'planar']
00052
00053 def __init__(self, name = None, child_link = None, parent_frame = None, joint_type = None):
00054 self.name = name
00055 self.child_link = child_link
00056 self.parent_frame = parent_frame
00057 self.type = joint_type
00058
00059 def check_valid(self):
00060 assert self.type in self.TYPES, "Invalid joint type: {}".format(self.type)
00061
00062
00063 @property
00064 def joint_type(self): return self.type
00065 @joint_type.setter
00066 def joint_type(self, value): self.type = value
00067
00068 xmlr.reflect(VirtualJoint, params = [
00069 name_attribute,
00070 xmlr.Attribute('child_link', str),
00071 xmlr.Attribute('parent_frame', str),
00072 xmlr.Attribute('type', str)
00073 ])
00074
00075
00076 class Chain(xmlr.Object):
00077 def __init__(self, base_link = None, tip_link = None):
00078 self.base_link = base_link
00079 self.tip_link = tip_link
00080
00081 xmlr.reflect(Chain, params = [
00082 xmlr.Attribute('base_link', str),
00083 xmlr.Attribute('tip_link', str)
00084 ])
00085
00086 class EndEffector(xmlr.Object):
00087 def __init__(self, name = None, group = None, parent_link = None, parent_group = None):
00088 self.name = name
00089 self.group = group
00090 self.parent_link = parent_link
00091 self.parent_group = parent_group
00092
00093 xmlr.reflect(EndEffector, params = [
00094 name_attribute,
00095 xmlr.Attribute('group', str),
00096 xmlr.Attribute('parent_link', str),
00097 xmlr.Attribute('parent_group', str, False)
00098 ])
00099
00100 class PassiveJoint(xmlr.Object):
00101 def __init__(self, name = None):
00102 self.name = name
00103
00104 xmlr.reflect(PassiveJoint, params = [
00105 name_attribute
00106 ])
00107
00108 class DisableCollisions(xmlr.Object):
00109 def __init__(self, link1 = None, link2 = None, reason = None):
00110 self.link1 = link1
00111 self.link2 = link2
00112 self.reason = reason
00113
00114 xmlr.reflect(DisableCollisions, params = [
00115 xmlr.Attribute('link1', str),
00116 xmlr.Attribute('link2', str),
00117 xmlr.Attribute('reason', str, False)
00118 ])
00119
00120
00121 class Group(xmlr.Object):
00122 def __init__(self, name = None):
00123 self.aggregate_init()
00124 self.name = name
00125 self.links = []
00126 self.joints = []
00127 self.chains = []
00128 self.groups = []
00129 self.subgroups = self.groups
00130
00131 xmlr.reflect(Group, params = [
00132 name_attribute,
00133 xmlr.AggregateElement('link', Link),
00134 xmlr.AggregateElement('joint', Joint),
00135 xmlr.AggregateElement('chain', Chain),
00136 xmlr.AggregateElement('group', Group)
00137 ])
00138
00139 class GroupState(xmlr.Object):
00140 def __init__(self, name = None, group = None):
00141 self.aggregate_init()
00142 self.name = name
00143 self.joints = []
00144 self.group = group
00145
00146 xmlr.reflect(GroupState, params = [
00147 name_attribute,
00148 xmlr.AggregateElement('joint', JointVal),
00149 xmlr.Attribute('group', str)
00150 ])
00151
00152
00153 class LinkSphereApproximation(xmlr.Object):
00154 def __init__(self, link = None):
00155 self.aggregate_init()
00156 self.link = link
00157 self.spheres = []
00158
00159 xmlr.reflect(LinkSphereApproximation, params = [
00160 xmlr.Attribute('link', str),
00161 xmlr.AggregateElement('sphere', Sphere)
00162 ])
00163
00164 class Robot(xmlr.Object):
00165 def __init__(self, name = None):
00166 self.aggregate_init()
00167
00168 self.name = name
00169 self.groups = []
00170 self.group_states = []
00171 self.end_effectors = []
00172 self.virtual_joints = []
00173 self.disable_collisionss = []
00174 self.passive_joints = []
00175 self.link_sphere_approximations = []
00176 self.group_map = {}
00177 self.group_state_map = {}
00178
00179 def add_aggregate(self, typeName, elem):
00180 xmlr.Object.add_aggregate(self, typeName, elem)
00181
00182 if typeName == 'group':
00183 group = elem
00184 self.group_map[group.name] = group
00185 elif typeName == 'group_state':
00186 group_state = elem
00187 self.group_state_map[group_state.name] = group_state
00188
00189 def add_link(self, link):
00190 self.add_aggregate('link', link)
00191
00192 def add_joint(self, joint):
00193 self.add_aggregate('joint', joint)
00194
00195 def add_chain(self, chain):
00196 self.add_aggregate('chain', chain)
00197
00198 def add_group(self, group):
00199 self.add_aggregate('group', group)
00200
00201 def add_passive_joint(self, joint):
00202 self.add_aggregate('passive_joint', joint)
00203
00204 def add_disable_collisions(self, col):
00205 self.add_aggregate('disable_collisions', col)
00206
00207 def add_link_sphere_approximation(self, link):
00208 self.add_aggregate('link_sphere_approximation', link)
00209
00210
00211 @classmethod
00212 def from_parameter_server(cls, key = 'robot_description_semantic'):
00213 """
00214 Retrieve the robot semantic model on the parameter server
00215 and parse it to create a SRDF robot structure.
00216
00217 Warning: this requires roscore to be running.
00218 """
00219
00220 import rospy
00221 return cls.from_xml_string(rospy.get_param(key))
00222
00223 xmlr.reflect(Robot, tag = 'robot', params = [
00224
00225 xmlr.Attribute('name', str, False),
00226 xmlr.AggregateElement('group', Group),
00227 xmlr.AggregateElement('group_state', GroupState),
00228 xmlr.AggregateElement('end_effector', EndEffector),
00229 xmlr.AggregateElement('virtual_joint', VirtualJoint),
00230 xmlr.AggregateElement('passive_joint', PassiveJoint),
00231 xmlr.AggregateElement('disable_collisions', DisableCollisions),
00232 xmlr.AggregateElement('link_sphere_approximation', LinkSphereApproximation)
00233 ])
00234
00235
00236 SRDF = Robot
00237
00238 xmlr.end_namespace()