sdf.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 # What is the scope of plugins? Model, World, Sensor?
00005 
00006 xmlr.start_namespace('sdf')
00007 
00008 
00009 class Pose(xmlr.Object):
00010     def __init__(self, vec=None, extra=None):
00011         self.xyz = None
00012         self.rpy = None
00013         if vec is not None:
00014             assert isinstance(vec, list)
00015             count = len(vec)
00016             if len == 3:
00017                 xyz = vec
00018             else:
00019                 self.from_vec(vec)
00020         elif extra is not None:
00021             assert xyz is None, "Cannot specify 6-length vector and 3-length vector"  # noqa
00022             assert len(extra) == 3, "Invalid length"
00023             self.rpy = extra
00024 
00025     def from_vec(self, vec):
00026         assert len(vec) == 6, "Invalid length"
00027         self.xyz = vec[:3]
00028         self.rpy = vec[3:6]
00029 
00030     def as_vec(self):
00031         xyz = self.xyz if self.xyz else [0, 0, 0]
00032         rpy = self.rpy if self.rpy else [0, 0, 0]
00033         return xyz + rpy
00034 
00035     def read_xml(self, node):
00036         # Better way to do this? Define type?
00037         vec = get_type('vector6').read_xml(node)
00038         self.load_vec(vec)
00039 
00040     def write_xml(self, node):
00041         vec = self.as_vec()
00042         get_type('vector6').write_xml(node, vec)
00043 
00044     def check_valid(self):
00045         assert self.xyz is not None or self.rpy is not None
00046 
00047 
00048 name_attribute = xmlr.Attribute('name', str)
00049 pose_element = xmlr.Element('pose', Pose, False)
00050 
00051 
00052 class Entity(xmlr.Object):
00053     def __init__(self, name=None, pose=None):
00054         self.name = name
00055         self.pose = pose
00056 
00057 
00058 xmlr.reflect(Entity, params=[
00059     name_attribute,
00060     pose_element
00061 ])
00062 
00063 
00064 class Inertia(xmlr.Object):
00065     KEYS = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']
00066 
00067     def __init__(self, ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0):
00068         self.ixx = ixx
00069         self.ixy = ixy
00070         self.ixz = ixz
00071         self.iyy = iyy
00072         self.iyz = iyz
00073         self.izz = izz
00074 
00075     def to_matrix(self):
00076         return [
00077             [self.ixx, self.ixy, self.ixz],
00078             [self.ixy, self.iyy, self.iyz],
00079             [self.ixz, self.iyz, self.izz]]
00080 
00081 
00082 xmlr.reflect(Inertia,
00083              params=[xmlr.Element(key, float) for key in Inertia.KEYS])
00084 
00085 # Pretty much copy-paste... Better method?
00086 # Use multiple inheritance to separate the objects out so they are unique?
00087 
00088 
00089 class Inertial(xmlr.Object):
00090     def __init__(self, mass=0.0, inertia=None, pose=None):
00091         self.mass = mass
00092         self.inertia = inertia
00093         self.pose = pose
00094 
00095 
00096 xmlr.reflect(Inertial, params=[
00097     xmlr.Element('mass', float),
00098     xmlr.Element('inertia', Inertia),
00099     pose_element
00100 ])
00101 
00102 
00103 class Link(Entity):
00104     def __init__(self, name=None, pose=None, inertial=None, kinematic=False):
00105         Entity.__init__(self, name, pose)
00106         self.inertial = inertial
00107         self.kinematic = kinematic
00108 
00109 
00110 xmlr.reflect(Link, parent_cls=Entity, params=[
00111     xmlr.Element('inertial', Inertial),
00112     xmlr.Attribute('kinematic', bool, False),
00113     xmlr.AggregateElement('visual', Visual, var='visuals'),
00114     xmlr.AggregateElement('collision', Collision, var='collisions')
00115 ])
00116 
00117 
00118 class Model(Entity):
00119     def __init__(self, name=None, pose=None):
00120         Entity.__init__(self, name, pose)
00121         self.links = []
00122         self.joints = []
00123         self.plugins = []
00124 
00125 
00126 xmlr.reflect(Model, parent_cls=Entity, params=[
00127     xmlr.AggregateElement('link', Link, var='links'),
00128     xmlr.AggregateElement('joint', Joint, var='joints'),
00129     xmlr.AggregateElement('plugin', Plugin, var='plugins')
00130 ])
00131 
00132 xmlr.end_namespace('sdf')


urdfdom_py
Author(s): Thomas Moulard, David Lu, Kelsey Hawkins, Antonio El Khoury, Eric Cousineau, Ioan Sucan , Jackie Kay
autogenerated on Thu Jun 6 2019 20:07:42