[Pyode-commits] pyode/xode joint.py,1.2,1.3

[Timothy S. <pe...@us...>]

Update of /cvsroot/pyode/pyode/xode
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv19119/xode

Modified Files:
	joint.py 
Log Message:
Added remaining joint types to the XODE parser.


Index: joint.py
===================================================================
RCS file: /cvsroot/pyode/pyode/xode/joint.py,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** joint.py	16 Jul 2004 11:45:17 -0000	1.2
--- joint.py	6 Sep 2004 05:56:55 -0000	1.3
***************
*** 86,103 ****
              pass
          elif (name == 'amotor'):
!             pass
          elif (name == 'ball'):
              l1, l2 = self._getLinks()
              self._parseBallJoint(self._world, l1, l2)
          elif (name == 'fixed'):
!             pass
          elif (name == 'hinge'):
!             pass
          elif (name == 'hinge2'):
!             pass
          elif (name == 'slider'):
!             pass
          elif (name == 'universal'):
!             pass
          else:
              raise errors.ChildError('joint', name)
--- 86,109 ----
              pass
          elif (name == 'amotor'):
!             l1, l2 = self._getLinks()
!             self._parseAMotor(self._world, l1, l2)
          elif (name == 'ball'):
              l1, l2 = self._getLinks()
              self._parseBallJoint(self._world, l1, l2)
          elif (name == 'fixed'):
!             l1, l2 = self._getLinks()
!             self._parseFixedJoint(self._world, l1, l2)
          elif (name == 'hinge'):
!             l1, l2 = self._getLinks()
!             self._parseHingeJoint(self._world, l1, l2)
          elif (name == 'hinge2'):
!             l1, l2 = self._getLinks()
!             self._parseHinge2Joint(self._world, l1, l2)
          elif (name == 'slider'):
!             l1, l2 = self._getLinks()
!             self._parseSliderJoint(self._world, l1, l2)
          elif (name == 'universal'):
!             l1, l2 = self._getLinks()
!             self._parseUniversalJoint(self._world, l1, l2)
          else:
              raise errors.ChildError('joint', name)
***************
*** 109,112 ****
--- 115,140 ----
              self._parser.pop()
  
+     def _applyAxisParams(self, joint, anum, axis):
+         def setParam(name):
+             attr = 'Param%s' % name
+             if (anum != 0):
+                 attr = '%s%i' % (attr, anum+1)
+             
+             joint.setParam(getattr(ode, attr), float(axis[name]))
+ 
+         if (axis.has_key('LowStop')):
+             axis['LoStop'] = axis['LowStop']
+             del axis['LowStop']
+             
+         for name in axis.keys():
+             if (name not in ['x', 'y', 'z']):
+                 if (name in ['LoStop', 'HiStop', 'Vel', 'FMax', 'FudgeFactor',
+                              'Bounce', 'CFM', 'StopERP', 'StopCFM',
+                              'SuspensionERP', 'SuspensionCFM']):
+                     setParam(name)
+                 else:
+                     raise errors.InvalidError('Invalid attribute %s' % `name` +
+                                               ' of <axis> element.')
+         
      def _parseBallJoint(self, world, link1, link2):
          anchor = [None]
***************
*** 129,130 ****
--- 157,340 ----
      
          self._parser.push(startElement=start, endElement=end)
+ 
+     def _parseFixedJoint(self, world, link1, link2):
+         
+         def start(name, attrs):
+             raise errors.ChildError('fixed', name)
+     
+         def end(name):
+             if (name == 'fixed'):
+                 self._parser.pop()
+         
+         joint = ode.FixedJoint(world, self._jg)
+         joint.attach(link1, link2)
+         self.setODEObject(joint)
+ 
+         self._parser.push(startElement=start, endElement=end)
+ 
+     def _parseHingeJoint(self, world, link1, link2):
+         anchor = [None]
+         axes = []
+     
+         def start(name, attrs):
+             if (name == 'anchor'):
+                 anchor[0] = self._parser.parseVector(attrs)
+             elif (name == 'axis'):
+                 axes.append(attrs)
+             else:
+                 raise errors.ChildError('hinge', name)
+     
+         def end(name):
+             if (name == 'hinge'):
+                 joint = ode.HingeJoint(world, self._jg)
+                 joint.attach(link1, link2)
+                 
+                 if (anchor[0] is not None):
+                     joint.setAnchor(anchor[0])
+ 
+                 if (len(axes) != 1):
+                     raise errors.InvalidError('Wrong number of axes for hinge'
+                                               ' joint.')
+                 
+                 joint.setAxis(self._parser.parseVector(axes[0]))
+                 self._applyAxisParams(joint, 0, axes[0])
+                 
+                 self.setODEObject(joint)
+                 self._parser.pop()
+     
+         self._parser.push(startElement=start, endElement=end)
+ 
+     def _parseSliderJoint(self, world, link1, link2):
+         axes = []
+     
+         def start(name, attrs):
+             if (name == 'axis'):
+                 axes.append(attrs)
+             else:
+                 raise errors.ChildError('slider', name)
+     
+         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()
+     
+         self._parser.push(startElement=start, endElement=end)
+ 
+     def _parseUniversalJoint(self, world, link1, link2):
+         anchor = [None]
+         axes = []
+     
+         def start(name, attrs):
+             if (name == 'anchor'):
+                 anchor[0] = self._parser.parseVector(attrs)
+             elif (name == 'axis'):
+                 axes.append(attrs)
+             else:
+                 raise errors.ChildError('universal', name)
+     
+         def end(name):
+             if (name == 'universal'):
+                 joint = ode.UniversalJoint(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 '
+                                               ' universal 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()
+     
+         self._parser.push(startElement=start, endElement=end)
+ 
+     def _parseHinge2Joint(self, world, link1, link2):
+         anchor = [None]
+         axes = []
+     
+         def start(name, attrs):
+             if (name == 'anchor'):
+                 anchor[0] = self._parser.parseVector(attrs)
+             elif (name == 'axis'):
+                 axes.append(attrs)
+             else:
+                 raise errors.ChildError('hinge2', name)
+     
+         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()
+     
+         self._parser.push(startElement=start, endElement=end)
+ 
+     def _parseAMotor(self, world, link1, link2):
+         anchor = [None]
+         axes = []
+     
+         def start(name, attrs):
+             # The XODE specification allows anchor elements for AMotor but
+             # there is no way to set the anchor of an AMotor.
+             
+             #if (name == 'anchor'):
+             #    anchor[0] = self._parser.parseVector(attrs)
+             
+             if (name == 'axis'):
+                 axes.append(attrs)
+             else:
+                 raise errors.ChildError('amotor', name)
+     
+         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()
+     
+         self._parser.push(startElement=start, endElement=end)