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]
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]
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]]
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]]
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()