def Callback(self, data, o1, o2):
o1IsGround = addressof(self.__groundGeom.contents) == addressof(o1.contents)
o2IsGround = addressof(self.__groundGeom.contents) == addressof(o2.contents)
if not (o1IsGround or o2IsGround):
return
o1IsBall = addressof(self.__ballGeom.contents) == addressof(o1.contents)
o2IsBall = addressof(self.__ballGeom.contents) == addressof(o2.contents)
if not (o1IsBall or o2IsBall):
return
self.__count += 1
ballGeom = o1 if o1IsBall else o2
ballBody = dGeomGetBody(ballGeom)
r = dGeomSphereGetRadius(ballGeom)
z = dBodyGetPosition(ballBody)[2]
if not (0 <= z and z <= r):
self.__isError = True
return
N = 10
contacts = (dContact * N)()
n = dCollide(o1, o2, N, byref(contacts[0].geom), sizeof(dContact))
if not n == 1:
self.__isError = True
return
contact = contacts[0]
contact.surface.mu = float('inf')
contact.surface.mode = dContactBounce
contact.surface.bounce = 0.95
contact.surface.bounce_vel = 0.0
c = dJointCreateContact(self.__world, self.__contactgroup, byref(contact))
dJointAttach(c, dGeomGetBody(contact.geom.g1), dGeomGetBody(contact.geom.g2))
def robotGeom(self, world, ballGeom, legGeom):
ballBody = dGeomGetBody(ballGeom)
legBody = dGeomGetBody(legGeom)
pos = [0.0, 0.0, 2.5]
dBodySetPosition(ballBody, pos[0], pos[1], pos[2])
rBall = dGeomSphereGetRadius(ballGeom)
rLeg = dReal()
lLeg = dReal()
dGeomCapsuleGetParams(legGeom, byref(rLeg), byref(lLeg))
dBodySetPosition(legBody, pos[0], pos[1],
pos[2] - (rBall + lLeg.value / 2.0))
joint = dJointCreateHinge(world, 0)
dJointAttach(joint, ballBody, legBody)
dJointSetHingeAnchor(joint, pos[0], pos[1], pos[2] - rBall)
dJointSetHingeAxis(joint, 1, 0, 0)
dJointSetHingeParam(joint, dParamLoStop, 0)
dJointSetHingeParam(joint, dParamHiStop, 0)
# pi = 3.14159
# dJointSetHingeParam(joint, dParamLoStop, -pi / 2.0)
# dJointSetHingeParam(joint, dParamHiStop, pi / 2.0)
# dJointSetHingeParam(joint, dParamLoStop, -pi)
# dJointSetHingeParam(joint, dParamHiStop, pi)
return ballGeom, legGeom
def __StepCallback(self, pause):
if self.__beforeStepCallback is not None:
self.__beforeStepCallback()
dSpaceCollide(self.__space, 0, dNearCallback(self.__nearCallback))
dWorldStep(self.__world, self.__tDelta)
dJointGroupEmpty(self.__contactgroup)
for geom, color in zip(self.__geoms, self.__geomColors):
body = dGeomGetBody(geom)
if dGeomGetClass(geom) == dSphereClass:
r = dGeomSphereGetRadius(geom)
pos = dBodyGetPosition(body)
rot = dBodyGetRotation(body)
dsSetColor(*color)
dsDrawSphereD(pos, rot, r)
elif dGeomGetClass(geom) == dCapsuleClass:
r = dReal()
l = dReal()
dGeomCapsuleGetParams(geom, byref(r), byref(l))
pos = dBodyGetPosition(body)
rot = dBodyGetRotation(body)
dsSetColor(*color)
dsDrawCapsuleD(pos, rot, l.value, r.value)
else:
raise DrawstuffError('Not supported geom class: {}'.format(
dGeomGetClass(geom)))
def robot(self, world, ballGeom, legGeoms):
ballBody = dGeomGetBody(ballGeom)
legBodies = [dGeomGetBody(legGeom) for legGeom in legGeoms]
rBall = dGeomSphereGetRadius(ballGeom)
lLegs = []
for legGeom in legGeoms:
rLeg = dReal()
lLeg = dReal()
dGeomCapsuleGetParams(legGeom, byref(rLeg), byref(lLeg))
lLegs.append(lLeg.value)
pos = [0.0, 0.0, 1.5]
dBodySetPosition(ballBody, pos[0], pos[1], pos[2])
for i, legBody in enumerate(legBodies):
if i == 0:
z = pos[2] - (rBall + lLegs[0] / 2.0)
dBodySetPosition(legBody, pos[0], pos[1], z)
elif i == 1:
z = pos[2] - (rBall + lLegs[0])
dBodySetPosition(legBody, pos[0], pos[1], z)
else:
raise Exception('robot has two leg geoms.')
jointSlider = dJointCreateSlider(world, 0)
dJointAttach(jointSlider, legBodies[0], legBodies[1])
dJointSetSliderAxis(jointSlider, 0, 0, 1)
dJointSetSliderParam(jointSlider, dParamLoStop, -lLegs[1] / 2.0)
dJointSetSliderParam(jointSlider, dParamHiStop, lLegs[1] / 2.0)
jointHinge = dJointCreateHinge(world, 0)
dJointAttach(jointHinge, ballBody, legBodies[0])
dJointSetHingeAnchor(jointHinge, pos[0], pos[1], pos[2] - rBall)
dJointSetHingeAxis(jointHinge, 1, 0, 0)
dJointSetHingeParam(jointHinge, dParamLoStop, 0)
dJointSetHingeParam(jointHinge, dParamHiStop, 0)
return {
'geoms': [ballGeom, legGeoms[0], legGeoms[1]],
'joints': [jointSlider, jointHinge],
}
def Callback(self, data, o1, o2):
o1IsGround = addressof(self.__groundGeom.contents) == addressof(
o1.contents)
o2IsGround = addressof(self.__groundGeom.contents) == addressof(
o2.contents)
if not (o1IsGround or o2IsGround):
return
N = 10
contacts = (dContact * N)()
n = dCollide(o1, o2, N, byref(contacts[0].geom), sizeof(dContact))
for i in range(n):
contact = contacts[i]
contact.surface.mu = float('inf')
contact.surface.mode = dContactBounce
contact.surface.bounce = 0.95
contact.surface.bounce_vel = 0.0
c = dJointCreateContact(self.__world, self.__contactgroup,
byref(contact))
dJointAttach(c, dGeomGetBody(contact.geom.g1),
dGeomGetBody(contact.geom.g2))