def create(self):
self.body = ode.Body(self.world)
mass = ode.Mass()
mass.setCylinderTotal(75, 2, PERSONRADIUS, PERSONHEIGHT)
geom = ode.GeomCylinder(self.space, PERSONRADIUS, PERSONHEIGHT)
geom.setBody(self.body)
x, y, z = self.position
self.body.setRotation((1, 0, 0, 0, 0, -1, 0, 1, 0))
self.body.setPosition((x, y + PERSONHEIGHT / 2 + SPHERERADIUS, z))
self.viz.addGeom(geom)
self.viz.GetProperty(self.body).SetColor(PERSONCOLOR)
#create spheres
self.w1, gw1 = self.createSphere(x + PERSONRADIUS - SPHERERADIUS, z)
self.w2, gw2 = self.createSphere(x - PERSONRADIUS + SPHERERADIUS, z)
#s3, gs3 = self.createSphere(x, z + PERSONRADIUS - SPHERERADIUS)
self.s4, gs4 = self.createSphere(x, z - PERSONRADIUS + SPHERERADIUS)
#create joints
self.j1 = ode.Hinge2Joint(self.world)
self.j1.attach(self.w1, self.body)
self.j1.setAnchor((x + PERSONRADIUS - SPHERERADIUS, SPHERERADIUS, z))
self.j1.setAxis1(self.axis3)
self.j1.setAxis2(self.axis2)
self.j2 = ode.Hinge2Joint(self.world)
self.j2.attach(self.w2, self.body)
self.j2.setAnchor((x - PERSONRADIUS + SPHERERADIUS, SPHERERADIUS, z))
self.j2.setAxis1(self.axis3)
self.j2.setAxis2(self.axis2)
'''j3 = ode.Hinge2Joint(self.world)
j3.attach(s3, self.body)
j3.setAnchor((x, SPHERERADIUS, z + PERSONRADIUS - SPHERERADIUS))
j3.setAxis1(self.axis3)
j3.setAxis2(self.axis2)'''
j4 = ode.Hinge2Joint(self.world)
j4.attach(self.s4, self.body)
j4.setAnchor((x, SPHERERADIUS, z - PERSONRADIUS + SPHERERADIUS))
j4.setAxis1(self.axis3)
j4.setAxis2(self.axis2)
#add person and road to allGroups
allGroups.append([self.w1, self.body])
allGroups.append([self.w2, self.body])
#allGroups.append([s3, self.body])
allGroups.append([self.s4, self.body])
allGroups.append([self.w1, self.road])
allGroups.append([self.w2, self.road])
#allGroups.append([s3, self.road])
allGroups.append([self.s4, self.road])
self.setLinearVelocity()
def create_hinge2(self,
key,
pos,
bod_keys,
axis1=[1, 0, 0],
axis2=[0, 0, 1],
loStop1=-ode.Infinity,
hiStop1=ode.Infinity,
fmax=-1,
fmax2=-1):
""" Create a universal joint.
Arguments:
@param key : number id to assign the hinge
@param pos : position of the joint
@param bod_keys : list of two bodies to attach the joint to
@param axis1 : first axis of hinge
@param axis2 : second axis of hinge
@param loStop1 : low limit of hinge 1
@param hiStop1 : high limit of hinge 1
@param fmax : max output potential for first hinge
@param fmax2 : max output potential for second hinge
"""
# Auto label the joint key or not.
key = len(self.joints) if key == -1 else key
if (pos == "None"):
anchor = self.bodies[bod_keys[0]].getRelPointPos(
(0., -self.bodies[bod_keys[0]].length / 2., 0.))
else:
anchor = pos
j = ode.Hinge2Joint(self.world)
j.attach(self.bodies[bod_keys[0]], self.bodies[bod_keys[1]])
j.setAnchor((anchor))
j.setAxis1((axis1))
j.setAxis2((axis2))
j.setParam(ode.ParamLoStop,
loStop1 - (self.joint_cushion * ANG_TO_RAD))
j.setParam(ode.ParamHiStop,
hiStop1 + (self.joint_cushion * ANG_TO_RAD))
j.setParam(ode.ParamFudgeFactor, .5)
j.setParam(ode.ParamFudgeFactor2, .5)
if fmax == -1:
j.setParam(ode.ParamFMax, 10.)
else:
j.setParam(ode.ParamFMax, fmax)
if fmax2 == -1:
j.setParam(ode.ParamFMax2, 10.)
else:
j.setParam(ode.ParamFMax2, fmax2)
j.style = "hinge2"
self.joints[key] = j
return key
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 _createODEjoint(self):
# Create the ODE joint
self.odejoint = ode.Hinge2Joint(self.odedynamics.world)
def create(self):
#create body
self.bodyCar = ode.Body(self.world)
mCar = ode.Mass()
mCar.setBoxTotal((self.passengers * PERSONWEIGHT + CARWEIGHT),
CARLENGTH, CARHEIGHT, CARWIDTH - WHEELWIDTH)
self.bodyCar.setMass(mCar)
self.geomCar = ode.GeomBox(
self.space, (CARLENGTH, CARHEIGHT, CARWIDTH - WHEELWIDTH))
self.geomCar.setBody(self.bodyCar)
self.bodyCar.setPosition(
(0 + self.position[0],
WHEELRADIUS + CARHEIGHT / 2 + self.position[1],
0 + self.position[2]))
self.viz.addGeom(self.geomCar)
self.viz.GetProperty(self.bodyCar).SetColor(self.bodyCarColor)
#create wheels
self.bodyFWL, self.geomFWL = self.createWheels(CARLENGTH / 2,
-CARWIDTH / 2)
self.bodyFWR, self.geomFWR = self.createWheels(CARLENGTH / 2,
CARWIDTH / 2)
self.bodyRWL, self.geomRWL = self.createWheels(-CARLENGTH / 2,
-CARWIDTH / 2)
self.bodyRWR, self.geomRWR = self.createWheels(-CARLENGTH / 2,
CARWIDTH / 2)
#create front left joint
self.flJoint = ode.Hinge2Joint(self.world)
self.flJoint.attach(self.bodyCar, self.bodyFWL)
self.flJoint.setAnchor(
(CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
-CARWIDTH / 2 + self.position[2]))
self.flJoint.setAxis2((0, 0, 1))
self.flJoint.setAxis1((0, 1, 0))
self.flJoint.setParam(ode.ParamSuspensionCFM, 0.5)
self.flJoint.setParam(ode.ParamSuspensionERP, 0.8)
#create front right joint
self.frJoint = ode.Hinge2Joint(self.world)
self.frJoint.attach(self.bodyCar, self.bodyFWR)
self.frJoint.setAnchor(
(CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
CARWIDTH / 2 + self.position[2]))
self.frJoint.setAxis2((0, 0, 1))
self.frJoint.setAxis1((0, 1, 0))
self.frJoint.setParam(ode.ParamSuspensionCFM, 0.5)
self.frJoint.setParam(ode.ParamSuspensionERP, 0.8)
#create rear left joint
self.rlJoint = ode.HingeJoint(self.world)
self.rlJoint.attach(self.bodyCar, self.bodyRWL)
self.rlJoint.setAnchor(
(-CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
-CARWIDTH / 2 + self.position[2]))
self.rlJoint.setAxis((0, 0, 1))
#create rear right joint
self.rrJoint = ode.HingeJoint(self.world)
self.rrJoint.attach(self.bodyCar, self.bodyRWR)
self.rrJoint.setAnchor(
(-CARLENGTH / 2 + self.position[0], WHEELRADIUS + self.position[1],
CARWIDTH / 2 + self.position[2]))
self.rrJoint.setAxis((0, 0, 1))
# add Joints to allGroups
allGroups.append([self.bodyFWL, self.bodyCar])
allGroups.append([self.bodyFWR, self.bodyCar])
allGroups.append([self.bodyRWL, self.bodyCar])
allGroups.append([self.bodyRWR, self.bodyCar])
#add Wheels and Road to allGroups
allGroups.append([self.bodyFWL, self.road])
allGroups.append([self.bodyFWR, self.road])
allGroups.append([self.bodyRWL, self.road])
allGroups.append([self.bodyRWR, self.road])
#create front left motor roll
self.flMotorRoll = ode.AMotor(self.world)
self.flMotorRoll.attach(self.bodyCar, self.bodyRWL)
self.flMotorRoll.setNumAxes(1)
self.flMotorRoll.setAxis(0, 2, (0, 0, 1))
self.flMotorRoll.enable()
self.flMotorRoll.setParam(ode.ParamSuspensionCFM, 0.5)
self.flMotorRoll.setParam(ode.ParamSuspensionERP, 0.8)
#create front left motor yaw
self.flMotorYaw = ode.AMotor(self.world)
self.flMotorYaw.attach(self.bodyCar, self.bodyRWL)
self.flMotorYaw.setNumAxes(1)
self.flMotorYaw.setAxis(0, 2, (0, 1, 0))
self.flMotorYaw.enable()
self.flMotorYaw.setParam(ode.ParamSuspensionCFM, 0.5)
self.flMotorYaw.setParam(ode.ParamSuspensionERP, 0.8)
#create front right motor
self.frMotorRoll = ode.AMotor(self.world)
self.frMotorRoll.attach(self.bodyCar, self.bodyRWR)
self.frMotorRoll.setNumAxes(1)
self.frMotorRoll.setAxis(0, 2, (0, 0, 1))
self.frMotorRoll.enable()
self.frMotorRoll.setParam(ode.ParamSuspensionCFM, 0.5)
self.frMotorRoll.setParam(ode.ParamSuspensionERP, 0.8)
#create front right motor
self.frMotorYaw = ode.AMotor(self.world)
self.frMotorYaw.attach(self.bodyCar, self.bodyRWR)
self.frMotorYaw.setNumAxes(1)
self.frMotorYaw.setAxis(0, 2, (0, 1, 0))
self.frMotorYaw.enable()
self.frMotorYaw.setParam(ode.ParamSuspensionCFM, 0.5)
self.frMotorYaw.setParam(ode.ParamSuspensionERP, 0.8)
bodyCar = ode.Body(world) mCar = ode.Mass() mCar.setBoxTotal(800, 1.5, .10, 3.0) bodyCar.setMass(mCar) geomCar = ode.GeomBox(space, (1.5, .1, 3)) geomCar.setBody(bodyCar) #bodyCar.setPosition((0 + position[0], wheelRadius + carHeight/2 + position[1], 0 + position[2])) viz.addGeom(geomCar) viz.GetProperty(bodyCar).SetColor(0, 0, 1) w1, g1 = createWheels(-1, -1.5) w2, g2 = createWheels(1, -1.5) w3, g3 = createWheels(1, 1.5) w4, g4 = createWheels(-1, 1.5) j1 = ode.Hinge2Joint(world) j1.setAnchor((-1, 0, -1.5)) j1.setAxis1((0, 1, 0)) j1.setAxis2((-1, 0, 0)) j1.attach(bodyCar, w1) j2 = ode.Hinge2Joint(world) j2.setAnchor((1, 0, -1.5)) j2.setAxis1((0, 1, 0)) j2.setAxis2((-1, 0, 0)) j2.attach(bodyCar, w2) j3 = ode.Hinge2Joint(world) j3.setAnchor((1, 0, 1.5)) j3.setAxis1((0, 1, 0)) j3.setAxis2((1, 0, 0))