def _getName(self, name):
root = self.getRoot()
try:
link = root.namedChild(name).getODEObject()
except KeyError:
raise errors.InvalidError('Joint link must reference an already '\
'parsed body.')
if (not isinstance(link, ode.Body)):
raise errors.InvalidError('Joint link must reference a body.')
return link
def _startElement(self, name, attrs):
nodeName = attrs.get('name', None)
if (name == 'transform'):
t = transform.Transform()
t.takeParser(self._parser, self, attrs)
else:
self._applyTransform()
if (name == 'torque'):
self.getODEObject().setTorque(self._parser.parseVector(attrs))
elif (name == 'force'):
self.getODEObject().setForce(self._parser.parseVector(attrs))
elif (name == 'finiteRotation'):
mode = int(attrs['mode'])
try:
axis = (float(attrs['xaxis']),
float(attrs['yaxis']),
float(attrs['zaxis']))
except KeyError:
raise errors.InvalidError('finiteRotation element must have' \
' xaxis, yaxis and zaxis attributes')
if (mode not in [0, 1]):
raise errors.InvalidError('finiteRotation mode attribute must' \
' be either 0 or 1.')
self.getODEObject().setFiniteRotationMode(mode)
self.getODEObject().setFiniteRotationAxis(axis)
elif (name == 'linearVel'):
self.getODEObject().setLinearVel(self._parser.parseVector(attrs))
elif (name == 'angularVel'):
self.getODEObject().setAngularVel(self._parser.parseVector(attrs))
elif (name == 'mass'):
self._mass = Mass(nodeName, self)
self._mass.takeParser(self._parser)
elif (name == 'joint'):
j = joint.Joint(nodeName, self)
j.takeParser(self._parser)
elif (name == 'body'):
b = Body(nodeName, self, attrs)
b.takeParser(self._parser)
elif (name == 'geom'):
g = geom.Geom(nodeName, self)
g.takeParser(self._parser)
elif (name == 'transform'): # so it doesn't raise ChildError
pass
else:
raise errors.ChildError('body', name)
def _endElement(self, name):
if (name == 'geom'):
obj = self.getODEObject()
if (obj is None):
raise errors.InvalidError('No geom type element found.')
self._parser.pop()
def parseVector(self, attrs):
"""
Parses an element's attributes as a vector.
@return: The vector (x, y, z).
@rtype: tuple
@raise errors.InvalidError: If the attributes don't correspond to a
valid vector.
"""
try:
vec = float(attrs['x']), float(attrs['y']), float(attrs['z'])
except ValueError:
raise errors.InvalidError('Vector attributes must be numbers.')
except KeyError:
raise errors.InvalidError('Vector must have x, y and z attributes.')
else:
return vec
def end(name):
if (name == 'slider'):
joint = ode.SliderJoint(world, self._jg)
joint.attach(link1, link2)
if (len(axes) != 1):
raise errors.InvalidError('Wrong number of axes for slider'
' joint.')
joint.setAxis(self._parser.parseVector(axes[0]))
self._applyAxisParams(joint, 0, axes[0])
self.setODEObject(joint)
self._parser.pop()
def _getLinks(self):
body = self._body or ode.environment
if (self._link1 is not None):
link1 = self._getName(self._link1)
else:
link1 = body
body = ode.environment
if (self._link2 is not None):
link2 = self._getName(self._link2)
else:
link2 = body
if (link1 is link2):
raise errors.InvalidError('Joint requires two objects.')
return link1, link2
def __init__(self, name, parent, attrs):
node.TreeNode.__init__(self, name, parent)
world = parent.getFirstAncestor(ode.World)
self.setODEObject(ode.Body(world.getODEObject()))
enabled = attrs.get('enabled', 'true')
if (enabled not in ['true', 'false']):
raise errors.InvalidError("Enabled attribute must be either 'true'"\
" or 'false'.")
else:
if (enabled == 'false'):
self.getODEObject().disable()
gravitymode = int(attrs.get('gravitymode', 1))
if (gravitymode == 0):
self.getODEObject().setGravityMode(0)
self._mass = None
self._transformed = False
def end(name):
if (name == 'amotor'):
joint = ode.AMotor(world, self._jg)
joint.attach(link1, link2)
if (anchor[0] is not None):
joint.setAnchor(anchor[0])
if (len(axes) > 3):
raise errors.InvalidError('Wrong number of axes for '
' amotor joint.')
joint.setNumAxes(len(axes))
for i in range(len(axes)):
joint.setAxis(i, 0, self._parser.parseVector(axes[i]))
self._applyAxisParams(joint, i, axes[i])
self.setODEObject(joint)
self._parser.pop()
def end(name):
if (name == 'hinge2'):
joint = ode.Hinge2Joint(world, self._jg)
joint.attach(link1, link2)
if (anchor[0] is not None):
joint.setAnchor(anchor[0])
if (len(axes) != 2):
raise errors.InvalidError('Wrong number of axes for '
' hinge2 joint.')
joint.setAxis1(self._parser.parseVector(axes[0]))
self._applyAxisParams(joint, 0, axes[0])
joint.setAxis2(self._parser.parseVector(axes[1]))
self._applyAxisParams(joint, 1, axes[1])
self.setODEObject(joint)
self._parser.pop()
def _endElement(self, name):
if (name == 'joint'):
if (self.getODEObject() is None):
raise errors.InvalidError('No joint type element found.')
self._parser.pop()
def _startElement(self, name, attrs):
if (name == 'xode'):
self._root = Root(None, None)
self._root.takeParser(self)
else:
raise errors.InvalidError('Root element must be <xode>.')
