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 test_joint2(self, world, space, contactgroup, ground, robot):
nearCallback = NearCallbackBounceGround(world=world,
contactgroup=contactgroup,
groundGeom=ground)
jointSlider = robot['joints'][0]
dWorldSetERP(world, 1.0)
dWorldSetCFM(world, 0.0)
beforeStepCallbackTestJoint2 = BeforeStepCallbackTestJoint2(
jointSlider=jointSlider)
def commandCallback(cmd):
print('cmd={}'.format(cmd))
if self.debug:
from .utils.drawstuff import Drawstuff
Drawstuff(world=world,
geoms=robot['geoms'],
space=space,
contactgroup=contactgroup,
beforeStepCallback=beforeStepCallbackTestJoint2.Callback,
nearCallback=nearCallback.Callback,
commandCallback=commandCallback,
sphereQuality=3).Run()
tDelta = 0.01
for i in range(999):
dSpaceCollide(space, 0, dNearCallback(nearCallback.Callback))
assert (dWorldStep(world, tDelta) == 1)
dJointGroupEmpty(contactgroup)
def test_freefall(self, g, world, sphere):
z0 = 5.0
tDelta = 0.01
eps = 0.05
dBodySetPosition(sphere, 0.0, 0.0, z0)
for i in range(99):
pos = dBodyGetPosition(sphere)
rot = dBodyGetRotation(sphere)
vel = dBodyGetLinearVel(sphere)
t = tDelta * i
v = g[2] * t
z = z0 + (g[2] * t * t / 2.0)
for i in range(3):
if i < 2:
assert (pos[i] == 0)
assert (vel[i] == 0)
else:
assert (abs(pos[i] - z) < eps)
assert (abs(vel[i] - v) < eps)
for i in range(3):
for j in range(4):
if i == j:
assert (rot[4 * i + j] == 1.0)
else:
assert (rot[4 * i + j] == 0.0)
assert (dWorldStep(world, tDelta) == 1)
def test_bounce(self, world, space, ground, ball, contactgroup):
nearCallback = NearCallbackTestSpace(world=world, contactgroup=contactgroup, groundGeom=ground, ballGeom=ball['geom'])
tDelta = 0.01
z0 = 3.0
dBodySetPosition(ball['body'], 0, 0, z0)
if self.debug:
from .utils.drawstuff import Drawstuff
Drawstuff(world=world, geoms=[ball['geom']], space=space, contactgroup=contactgroup, nearCallback=nearCallback.Callback).Run()
for i in range(999):
dSpaceCollide(space, 0, dNearCallback(nearCallback.Callback))
assert(dWorldStep(world, tDelta) == 1)
dJointGroupEmpty(contactgroup)
assert(nearCallback.GetCount() > 0)
assert(not nearCallback.IsError())
def test_joint(self, world, space, contactgroup, ground, robotGeom):
nearCallback = NearCallbackBounceGround(world=world,
contactgroup=contactgroup,
groundGeom=ground)
if self.debug:
from .utils.drawstuff import Drawstuff
Drawstuff(world=world,
geoms=list(robotGeom),
space=space,
contactgroup=contactgroup,
nearCallback=nearCallback.Callback).Run()
tDelta = 0.01
for i in range(999):
dSpaceCollide(space, 0, dNearCallback(nearCallback.Callback))
assert (dWorldStep(world, tDelta) == 1)
dJointGroupEmpty(contactgroup)