Ejemplo n.º 1
0
class AngularMotor(Motor, Base):
  def __init__(self, actuated = None, **kargs):
    super(AngularMotor, self).__init__(**kargs)

    self.joint = Joint(type = "Hinge", limits = (self.minAngle,
      self.maxAngle), stopERP = 0.9, stopCFM = 0, fudgeFactor = 0.1)
    
    self.actuated = actuated
    
#-------------------------------------------------------------------------------

  def setActuated(self, actuated):
    if self.actuated and self.actuated.body:
      self.joint.detach()
      
    super(AngularMotor, self).setActuated(actuated)
    
    if self.actuated and self.actuated.body:
      self.joint.attach(self, self.actuated)
      self.joint.axis = [-self.axis[0], -self.axis[1], -self.axis[2]]
    
  actuated = property(Base.getActuated, setActuated)

#-------------------------------------------------------------------------------

  def move(self, period):
    Motor.move(self, period)
      
    self.joint.axisRate = self.angularVelocity
    self.joint.maxForce = self.axisForces[0]
    
Ejemplo n.º 2
0
class LinearMotor(Motor, Base):
  def __init__(self, actuated = None, **kargs):
    super(LinearMotor, self).__init__(**kargs)

    self.joint = Joint(type = "Slider", limits = (self.minPosition,
      self.maxPosition), stopERP = 0.9, stopCFM = 0)

    self.actuated = actuated
    
#-------------------------------------------------------------------------------

  def setActuated(self, actuated):
    if self.actuated and self.actuated.body:
      self.joint.detach()
      
    super(LinearMotor, self).setActuated(actuated)
    
    if self.actuated and self.actuated.body:
      self.joint.attach(self, self.actuated)
      self.joint.axis = [-self.axis[0], -self.axis[1], -self.axis[2]]
    
  actuated = property(Base.getActuated, setActuated)

#-------------------------------------------------------------------------------

  def move(self, period):
    Motor.move(self, period)
    
    self.joint.axisVelocity = self.linearVelocity
    self.joint.maxForce = self.axisForces[0]
    
Ejemplo n.º 3
0
  def __init__(self, actuated = None, **kargs):
    super(PlanarMotor, self).__init__(**kargs)

    linearLimits = [(self.minPosition[0], self.maxPosition[0]),
                    (self.minPosition[1], self.maxPosition[1]),
                    (0, 0)]
    angularLimits = [(0, 0), (0, 0), (self.minAngle, self.maxAngle)]
                   
    self.linearJoint = Joint(type = "LinearMotor", limits =
      linearLimits, stopERP = 0.9, stopCFM = 0)
    self.angularJoint = Joint(type = "AngularMotor", limits =
      angularLimits, stopERP = 0.9, stopCFM = 0, fudgeFactor = 0.1)

    self.actuated = actuated
Ejemplo n.º 4
0
  def __init__(self, actuated = None, **kargs):
    super(AngularMotor, self).__init__(**kargs)

    self.joint = Joint(type = "Hinge", limits = (self.minAngle,
      self.maxAngle), stopERP = 0.9, stopCFM = 0, fudgeFactor = 0.1)
    
    self.actuated = actuated
Ejemplo n.º 5
0
  def __init__(self, actuated = None, **kargs):
    super(LinearMotor, self).__init__(**kargs)

    self.joint = Joint(type = "Slider", limits = (self.minPosition,
      self.maxPosition), stopERP = 0.9, stopCFM = 0)

    self.actuated = actuated
Ejemplo n.º 6
0
class PlanarMotor(Motor, Base):
  def __init__(self, actuated = None, **kargs):
    super(PlanarMotor, self).__init__(**kargs)

    linearLimits = [(self.minPosition[0], self.maxPosition[0]),
                    (self.minPosition[1], self.maxPosition[1]),
                    (0, 0)]
    angularLimits = [(0, 0), (0, 0), (self.minAngle, self.maxAngle)]
                   
    self.linearJoint = Joint(type = "LinearMotor", limits =
      linearLimits, stopERP = 0.9, stopCFM = 0)
    self.angularJoint = Joint(type = "AngularMotor", limits =
      angularLimits, stopERP = 0.9, stopCFM = 0, fudgeFactor = 0.1)

    self.actuated = actuated
    
#-------------------------------------------------------------------------------

  def setActuated(self, actuated):
    if self.actuated and self.actuated.body:
      self.linearJoint.detach()
      self.angularJoint.detach()
      
    super(PlanarMotor, self).setActuated(actuated)
    
    if self.actuated and self.actuated.body:
      self.linearJoint.attach(self, self.actuated)
      self.angularJoint.attach(self, self.actuated)
      
      axes = [panda.Vec3(*self.axes[0]),
              panda.Vec3(*self.axes[1]),
              panda.Vec3(0, 0, 0)]
      
      axes[0].normalize()
      axes[1].normalize()
      axes[2] = axes[0].cross(axes[1])      
      
      self.linearJoint.axes = [-axes[0], -axes[1], -axes[2]]
      self.angularJoint.mode = 1
      self.angularJoint.axes = [-axes[0], -axes[1], -axes[2]]
      
  actuated = property(Base.getActuated, setActuated)

#-------------------------------------------------------------------------------

  def move(self, period):
    Motor.move(self, period)
    
    self.linearJoint.axisVelocities = [self.linearVelocity[0],
      self.linearVelocity[1], 0]
    self.linearJoint.maxForce = [self.axisForces[0],
      self.axisForces[1], float("inf")]
      
    self.angularJoint.axisAngles = [0, 0, self.axisAngle]
    self.angularJoint.axisRates = [0, 0, self.angularVelocity]
    self.angularJoint.maxForce = [float("inf"), float("inf"),
      self.axisForces[2]]
Ejemplo n.º 7
0
  def __init__(self, bearingSolid = None, bearingBody = None, bearingMass = 1,
      accelerationForce = [0, 0, 0], decelerationForce = [0, 0, 0], actuated =
      None, epsilon = 1e-6, **kargs):
    super(PlanarDrive, self).__init__(solid = bearingSolid, body = bearingBody,
      mass = bearingMass, **kargs)

    self.accelerationForce = accelerationForce
    self.decelerationForce = decelerationForce
    self.epsilon = epsilon
    
    self.solid.geometry.collide(self.collide)
    self.linearJoint = Joint(type = "LinearMotor", axes = [[1, 0, 0],
      [0, 1, 0]], stopERP = 0.9, stopCFM = 0)
    self.angularJoint = Joint(type = "AngularMotor", stopERP = 0.9,
      stopCFM = 0)

    self.actuated = actuated
Ejemplo n.º 8
0
 def setAnchor(self, anchor):
   if self._anchorJoint:
     self._anchorJoint.detach()
   
   Base.setAnchor(self, anchor)
   
   if self.anchor and self.body and self.anchor.body:
     if not self._anchorJoint:
       self._anchorJoint = Joint(type = "Fixed")
     self._anchorJoint.attach(self.anchor, self)
Ejemplo n.º 9
0
 def setBody(self, body):
   if self._body:
     if self._anchorJoint:
       self._anchorJoint.detach()
     self._body.detachNode()
   
   self._body = body
   
   if self._body:
     if self._body.object != self:
       self._body.object = self
     if self.anchor and self.anchor.body:
       if not self._anchorJoint:
         self._anchorJoint = Joint(type = "Fixed")
       self._anchorJoint.attach(self.anchor, self)
Ejemplo n.º 10
0
class PlanarDrive(Drive, Base):
  def __init__(self, bearingSolid = None, bearingBody = None, bearingMass = 1,
      accelerationForce = [0, 0, 0], decelerationForce = [0, 0, 0], actuated =
      None, epsilon = 1e-6, **kargs):
    super(PlanarDrive, self).__init__(solid = bearingSolid, body = bearingBody,
      mass = bearingMass, **kargs)

    self.accelerationForce = accelerationForce
    self.decelerationForce = decelerationForce
    self.epsilon = epsilon
    
    self.solid.geometry.collide(self.collide)
    self.linearJoint = Joint(type = "LinearMotor", axes = [[1, 0, 0],
      [0, 1, 0]], stopERP = 0.9, stopCFM = 0)
    self.angularJoint = Joint(type = "AngularMotor", stopERP = 0.9,
      stopCFM = 0)

    self.actuated = actuated
      
#-------------------------------------------------------------------------------

  def setActuated(self, actuated):
    if self.body:
      self.linearJoint.detach()
      self.angularJoint.detach()
      
    super(PlanarDrive, self).setActuated(actuated)
    
    if self.body:
      self.linearJoint.attach(self, None)
      self.angularJoint.attach(self, None)
    
  actuated = property(Base.getActuated, setActuated)
  
#-------------------------------------------------------------------------------

  def collide(self, contact):
    command = render.getRelativeVector(self,
      panda.Vec3(self.command[0], self.command[1], 0))
    command[2] = self.command[2]
    
    surface = contact.getSurface()
    surface.setMode(panda.OdeSurfaceParameters.MFContactApprox1 |
      panda.OdeSurfaceParameters.MFContactFDir1 |
      panda.OdeSurfaceParameters.MFContactMu2)
      
    if abs(command[0]) >= self.epsilon:
      surface.setMu2(0)
    if abs(command[1]) >= self.epsilon:
      surface.setMu(0)
    if abs(command[2]) >= self.epsilon:
      surface.setMu(0)
      surface.setMu2(0)
    
    contact.setSurface(surface)
    contact.setFdir1(render.getRelativeVector(self.solid,
      panda.Vec3(0, 1, 0)))

#-------------------------------------------------------------------------------

  def move(self, period):
    command = render.getRelativeVector(self,
      panda.Vec3(self.command[0], self.command[1], 0))
    command[2] = self.command[2]
    state = render.getRelativeVector(self,
      panda.Vec3(self.state[0], self.state[1], 0))
    state[2] = self.state[2]
    
    accelerationForce = render.getRelativeVector(self,
      panda.Vec3(self.accelerationForce[0], self.accelerationForce[1], 0))
    accelerationForce[2] = self.accelerationForce[2]
    decelerationForce = render.getRelativeVector(self,
      panda.Vec3(self.decelerationForce[0], self.decelerationForce[1], 0))
    decelerationForce[2] = self.decelerationForce[2]
    
    axisForces = [0]*len(self.command)
    
    for i in range(len(command)):
      velocityError = command[i]-state[i]
      
      if abs(velocityError) > self.epsilon:
        axisForces[i] = abs(accelerationForce[i])
      else:
        axisForces[i] = abs(decelerationForce[i])
        
    self.linearJoint.axisVelocities = [state[0], state[1]]
    self.linearJoint.maxForce = axisForces[0:2]
    self.angularJoint.axisRates = [0, 0, state[2]]
    self.angularJoint.maxForce = [float("inf"), float("inf"), axisForces[2]]
    
Ejemplo n.º 11
0
class Object(Base):
  def __init__(self, solid = None, body = None, anchor = None,
      collisionMasks = None, **kargs):
    self._solid = None
    self._body = None
    self._anchorJoint = None
    self._joints = []
    self._collisionMasks = None
    
    super(Object, self).__init__(**kargs)
    
    self.solid = solid
    self.body = body
    self.anchor = anchor
    self.collisionMasks = collisionMasks
 
#-------------------------------------------------------------------------------

  def getSolid(self):
    return self._solid
    
  def setSolid(self, solid):
    if self._solid:
      self._solid.detachNode()
    
    self._solid = solid
    
    if self._solid:
      if self._solid.object != self:
        self._solid.object = self
  
  solid = property(getSolid, setSolid)
  
#-------------------------------------------------------------------------------

  def getBody(self):
    return self._body
    
  def setBody(self, body):
    if self._body:
      if self._anchorJoint:
        self._anchorJoint.detach()
      self._body.detachNode()
    
    self._body = body
    
    if self._body:
      if self._body.object != self:
        self._body.object = self
      if self.anchor and self.anchor.body:
        if not self._anchorJoint:
          self._anchorJoint = Joint(type = "Fixed")
        self._anchorJoint.attach(self.anchor, self)
  
  body = property(getBody, setBody)
  
#-------------------------------------------------------------------------------

  def setAnchor(self, anchor):
    if self._anchorJoint:
      self._anchorJoint.detach()
    
    Base.setAnchor(self, anchor)
    
    if self.anchor and self.body and self.anchor.body:
      if not self._anchorJoint:
        self._anchorJoint = Joint(type = "Fixed")
      self._anchorJoint.attach(self.anchor, self)
    
  anchor = property(Base.getAnchor, setAnchor)
  
#-------------------------------------------------------------------------------

  def getJoints(self):
    return self._joints
    
  joints = property(getJoints)
  
#-------------------------------------------------------------------------------

  def getCollisionMasks(self):
    return self._collisionMasks
    
  def setCollisionMasks(self, collisionMasks):
    self._collisionMasks = collisionMasks
    
    if self._collisionMasks and self.solid and self.solid.geometry:
      self.solid.geometry.setCategoryBits(self._collisionMasks[0])
      self.solid.geometry.setCollideBits(self._collisionMasks[1])

  collisionMasks = property(getCollisionMasks, setCollisionMasks)
  
#-------------------------------------------------------------------------------

  def findJoint(self, object = None):
    for joint in self.joints:
      objects = joint.objects      
      if (objects[0] == object) or (objects[1] == object):
        return joint
    
    return None
  
#-------------------------------------------------------------------------------

  def showSolids(self, cameraMask = 0x10000000):
    objects = Iterator(self, Object).generator
    
    for object in objects:
      object.solid.show(panda.BitMask32(cameraMask))

#-------------------------------------------------------------------------------

  def hideSolids(self):
    objects = Iterator(self, Object).generator
    
    for object in objects:
      object.solid.hide(panda.BitMask32.allOn())

#-------------------------------------------------------------------------------

  def showBodies(self, cameraMask = 0x10000000):
    objects = Iterator(self, Object).generator
    
    for object in objects:
      object.body.show(panda.BitMask32(cameraMask))

#-------------------------------------------------------------------------------

  def hideBodies(self):
    objects = Iterator(self, Object).generator
    
    for object in objects:
      object.body.hide(panda.BitMask32.allOn())

#-------------------------------------------------------------------------------

  def showJoints(self, cameraMask = 0x10000000):
    objects = Iterator(self, Object).generator
    
    for object in objects:
      for joint in object.joints:
        joint.show(panda.BitMask32(cameraMask))

#-------------------------------------------------------------------------------

  def hideJoints(self):
    objects = Iterator(self, Object).generator
    
    for object in objects:
      for joint in object.joints:
        joint.hide(panda.BitMask32.allOn())

#-------------------------------------------------------------------------------

  def applyBodyTransform(self):
    if self.solid and self.solid.geometry and self.solid.placeable:
      self.solid.geometry.setPosition(*self.solid.globalPosition)
      self.solid.geometry.setQuaternion(self.solid.globalQuaternion)
    if self.body:
      self.body._body.setPosition(*self.body.globalPosition)
      self.body._body.setQuaternion(self.body.globalQuaternion)

    for joint in self.joints:
      objects = joint.objects
      if (objects[0] == self) and objects[1]:
        objects[1].applyBodyTransform()
        
#-------------------------------------------------------------------------------

  def onTranslate(self, translation):
    self.applyBodyTransform()
    
#-------------------------------------------------------------------------------

  def onRotate(self, rotation):
    self.applyBodyTransform()