srdf.py
Go to the documentation of this file.
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 # Common stuff
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 # Common stuff again
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   # Aliases
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     # Could move this into xml_reflection
00220     import rospy
00221     return cls.from_xml_string(rospy.get_param(key))
00222   
00223 xmlr.reflect(Robot, tag = 'robot', params = [
00224 #   name_attribute,
00225   xmlr.Attribute('name', str, False), # Is 'name' a required attribute?
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 # Make an alias
00236 SRDF = Robot
00237 
00238 xmlr.end_namespace()


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Thu Feb 9 2017 03:50:57