def test_Generic6DofConstraint_emulateP2P_sim(self, clsDof6):
"""
Test the Generic6Dof constraint in a Bullet simulation.
The 6DOF constraint in this test emulates a Point2Point constraint
because the default values for linear/angular motion are not modified.
The test code is therefore mostly identical to that for a Point2Point
constraint.
"""
# Create two rigid bodies side by side (they *do* touch, but just).
pos_a = Vec3(-1, 0, 0)
pos_b = Vec3(1, 0, 0)
rb_a = getRB(pos=pos_a, cshape=SphereShape(1))
rb_b = getRB(pos=pos_b, cshape=BoxShape(Vec3(1, 2, 3)))
# Create the constraint between the two bodies.
frameInA = Transform()
frameInB = Transform()
frameInA.setIdentity()
frameInB.setIdentity()
refIsA = True
dof = clsDof6(rb_a, rb_b, frameInA, frameInB, refIsA)
# Add both rigid bodies into a simulation.
bb = BulletBase()
bb.setGravity(Vec3(0, 0, 0))
bb.addRigidBody(rb_a)
bb.addRigidBody(rb_b)
# Add constraint to Bullet simulation.
bb.addConstraint(dof)
# Verify that the objects are at x-position +/-1, and thus 2 Meters
# apart.
p_a = rb_a.getCenterOfMassTransform().getOrigin().topy()
p_b = rb_b.getCenterOfMassTransform().getOrigin().topy()
init_pos = (p_a[0], p_b[0])
fixed_dist = p_a[0] - p_b[0]
assert init_pos == (-1, 1)
# Apply opposing forces to both objects, step the simulation a few
# times, and verify at each step that *both* objects move in the *same*
# direction due to the constraint.
rb_a.applyCentralForce(Vec3(10, 0, 0))
rb_b.applyCentralForce(Vec3(10, 0, 0))
for ii in range(3):
# Step simulation.
bb.stepSimulation(10 / 60, 60)
# Query the position of the objects.
p_a = rb_a.getCenterOfMassTransform().getOrigin().topy()
p_b = rb_b.getCenterOfMassTransform().getOrigin().topy()
# Verify that both objects continue to move to right, yet maintain
# their initial distance.
assert p_a[0] > init_pos[0]
assert p_b[0] > init_pos[1]
assert abs((p_a[0] - p_b[0]) - fixed_dist) < 0.1
init_pos = (p_a[0], p_b[0])
def test_Point2Point_sim(self):
"""
Test the Point2Point constraint in a Bullet simulation.
"""
# Create two rigid bodies side by side (they *do* touch, but just).
pos_a = Vec3(-1, 0, 0)
pos_b = Vec3(1, 0, 0)
rb_a = getRB(pos=pos_a, cshape=SphereShape(1))
rb_b = getRB(pos=pos_b, cshape=BoxShape(Vec3(1, 2, 3)))
# Connect the two rigid bodies at their left/right boundary.
pivot_a, pivot_b = pos_b, pos_a
p2p = Point2PointConstraint(rb_a, rb_b, pivot_a, pivot_b)
# Add both rigid bodies into a simulation.
bb = BulletBase()
bb.setGravity(Vec3(0, 0, 0))
bb.addRigidBody(rb_a)
bb.addRigidBody(rb_b)
# Tell the simulation about the constraint.
bb.addConstraint(p2p)
# Verify that the objects are at x-position +/-1, and thus 2 Meters
# apart.
p_a = rb_a.getCenterOfMassTransform().getOrigin().topy()
p_b = rb_b.getCenterOfMassTransform().getOrigin().topy()
init_pos = (p_a[0], p_b[0])
fixed_dist = p_a[0] - p_b[0]
assert init_pos == (-1, 1)
# Apply opposing forces to both objects, step the simulation a few
# times, and verify at each step that *both* objects move in the *same*
# direction due to the constraint.
rb_a.applyCentralForce(Vec3(10, 0, 0))
rb_b.applyCentralForce(Vec3(-1, 0, 0))
for ii in range(3):
# Step simulation.
bb.stepSimulation(10 / 60, 60)
# Query the position of the objects.
p_a = rb_a.getCenterOfMassTransform().getOrigin().topy()
p_b = rb_b.getCenterOfMassTransform().getOrigin().topy()
# Verify that both objects continue to move to right, yet maintain
# their initial distance.
assert p_a[0] > init_pos[0]
assert p_b[0] > init_pos[1]
assert abs((p_a[0] - p_b[0]) - fixed_dist) < 0.1
init_pos = (p_a[0], p_b[0])
def test_Generic6DofConstraint_emulateSlider_pivot_sim(self, clsDof6):
"""
Same as test_Generic6DofConstraint_emulateP2P_pivot_sim except
that the pivot does not coincide with the center of mass and the
constraint is setup such that it mimicks a slider.
"""
# Create two rigid bodies side by side (they *do* touch, but just).
pos_a = Vec3(-1, 0, 0)
pos_b = Vec3(1, 0, 0)
rb_a = getRB(pos=pos_a, cshape=SphereShape(1))
rb_b = getRB(pos=pos_b, cshape=BoxShape(Vec3(1, 2, 3)))
# Create the constraint between the two bodies. The constraint applies
# at (0, 0, 0) in world coordinates.
frameInA = Transform(Quaternion(0, 0, 0, 1), pos_b)
frameInB = Transform(Quaternion(0, 0, 0, 1), pos_a)
refIsA = True
dof = clsDof6(rb_a, rb_b, frameInA, frameInB, refIsA)
# We are now emulating a slider constraint with this 6DOF constraint.
# For this purpose we need to specify the linear/angular limits.
sliderLimitLo = -1
sliderLimitHi = 1
# Apply the linear/angular limits.
dof.setLinearLowerLimit(Vec3(sliderLimitLo, 0, 0))
dof.setLinearUpperLimit(Vec3(sliderLimitHi, 0, 0))
# Add both rigid bodies and the constraint to the Bullet simulation.
bb = BulletBase()
bb.setGravity(Vec3(0, 0, 0))
bb.addRigidBody(rb_a)
bb.addRigidBody(rb_b)
bb.addConstraint(dof)
# Verify that the objects are at x-position +/-1, and thus 2 Meters
# apart.
p_a = rb_a.getCenterOfMassTransform().getOrigin().topy()
p_b = rb_b.getCenterOfMassTransform().getOrigin().topy()
init_pos = (p_a[0], p_b[0])
assert init_pos == (-1, 1)
# Pull the right object to the right. Initially this must not affect
# the object on the left until the slider is fully extended, at which
# point the left object must begin to move as well.
for ii in range(5):
# Apply the force and step the simulation.
rb_b.applyCentralForce(Vec3(10, 0, 0))
bb.stepSimulation(10 / 60, 60)
# Query the position of the objects.
p_a = rb_a.getCenterOfMassTransform().getOrigin().topy()
p_b = rb_b.getCenterOfMassTransform().getOrigin().topy()
# If the right object has not moved far enough to fully extend the
# (emulated) slider constraint then the left object must remain
# where it is, otherwise it must move to the right.
if p_b[0] <= (init_pos[1] + sliderLimitHi):
assert p_a[0] == init_pos[0]
else:
assert p_a[0] > init_pos[0]
# Verify that the above loop really pulled the right object far enought
# to exhaust the maximum translation allowance.
assert p_b[0] > (init_pos[1] + sliderLimitHi)
def test_getLatestContacts(self):
def moveBody(rb, pos):
trans = Transform()
trans.setOrigin(pos)
ms = DefaultMotionState()
ms.setWorldTransform(trans)
rb.setMotionState(ms)
# Create two rigid bodies side by side (they *do* touch, but just).
pos_a = Vec3(-3, 0, 0)
pos_b = Vec3(-1, 0, 0)
pos_c = Vec3(1, 0, 0)
pos_d = Vec3(3, 0, 0)
rb_a = getRB(pos=pos_a, cshape=SphereShape(1), bodyID=1)
rb_b = getRB(pos=pos_b, cshape=SphereShape(1), bodyID=2)
rb_c = getRB(pos=pos_c, cshape=SphereShape(1), bodyID=3)
rb_d = getRB(pos=pos_d, cshape=SphereShape(1), bodyID=4)
# Create a simulation and add three objects. The relative positions of
# the objects is like this: "AB D", ie A & B are just touching, but D
# is by itself.
sim = BulletBase()
sim.addRigidBody(rb_a)
sim.addRigidBody(rb_b)
sim.addRigidBody(rb_d)
# Step the simulation and fetch the collision pairs.
sim.stepSimulation(1, 1)
ret = sim.azGetLastContacts()
assert set(ret.keys()) == set([(1, 2)])
# Move the middle body B towards the right so that touches D: "A BD".
moveBody(rb_a, pos_a)
moveBody(rb_b, pos_c)
moveBody(rb_d, pos_d)
sim.stepSimulation(1, 1)
ret = sim.azGetLastContacts()
assert set(ret.keys()) == set([(2, 4)])
# Move the B towards the far right so that it does not touch D at all:
# "A B D".
moveBody(rb_a, pos_a)
moveBody(rb_b, Vec3(30, 0, 0))
moveBody(rb_d, pos_d)
sim.stepSimulation(1, 1)
assert sim.azGetLastContacts() == {}
# Move the middle body back to its original position and insert the
# fourth body. Now all bodies touch their immediate neighbours:
# "ABCD"
sim.addRigidBody(rb_c)
moveBody(rb_a, pos_a)
moveBody(rb_b, pos_b)
moveBody(rb_c, pos_c)
moveBody(rb_d, pos_d)
sim.stepSimulation(1, 1)
ret = sim.azGetLastContacts()
assert set(ret.keys()) == set([(1, 2), (2, 3), (3, 4)])
def test_broadphase_collection(self):
def moveBody(rb, pos):
trans = Transform()
trans.setOrigin(pos)
ms = DefaultMotionState()
ms.setWorldTransform(trans)
rb.setMotionState(ms)
# Create two rigid bodies side by side (they *do* touch, but just).
pos_a = Vec3(-3, 0, 0)
pos_b = Vec3(-1, 0, 0)
pos_c = Vec3(1, 0, 0)
pos_d = Vec3(3, 0, 0)
rb_a = getRB(pos=pos_a, cshape=SphereShape(1), bodyID=1)
rb_b = getRB(pos=pos_b, cshape=SphereShape(1), bodyID=2)
rb_c = getRB(pos=pos_c, cshape=SphereShape(1), bodyID=3)
rb_d = getRB(pos=pos_d, cshape=SphereShape(1), bodyID=4)
# Create a simulation, install the Broadphase Pair Cache Builder, and
# add bodies A, B, D. The first two are touching, the last one is
# considerably away from the others. The bodies are arranged like this:
# "AB D"
sim = BulletBase()
sim.installBroadphaseCallback()
sim.addRigidBody(rb_a)
sim.addRigidBody(rb_b)
sim.addRigidBody(rb_d)
# Step the simulation and fetch the collision pairs.
sim.azResetPairCache()
assert sim.azReturnPairCache() == set([])
sim.stepSimulation(1, 1)
assert sim.azReturnPairCache() == set([(1, 2)])
# Move the middle body towards the right. Step the simulation again
# (this will re-populate the broadphase cache) and verify that the
# broadphase now returns the two objects on the right: "A BD"
moveBody(rb_b, pos_c)
sim.azResetPairCache()
sim.stepSimulation(1, 1)
assert sim.azReturnPairCache() == set([(2, 4)])
# Move the middle body towards the far right so that none of the bodies
# overlap: "A D B"
moveBody(rb_b, Vec3(30, 0, 0))
sim.azResetPairCache()
sim.stepSimulation(1, 1)
assert sim.azReturnPairCache() == set([])
# Move the middle body back to its original position and insert the
# fourth body. Now all bodies touch their immediate neighbours:
# "ABCD"
moveBody(rb_b, pos_b)
sim.addRigidBody(rb_c)
sim.azResetPairCache()
sim.stepSimulation(1, 1)
assert sim.azReturnPairCache() == set([(1, 2), (2, 3), (3, 4)])
def test_add_get_remove_iterate(self):
"""
Test the various functions
"""
# Create two rigid bodies side by side (they *do* touch, but just).
pos_a = Vec3(-3, 0, 0)
pos_b = Vec3(-1, 0, 0)
pos_c = Vec3(1, 0, 0)
pos_d = Vec3(3, 0, 0)
rb_a = getRB(pos=pos_a, cshape=SphereShape(1))
rb_b = getRB(pos=pos_b, cshape=BoxShape(Vec3(1, 2, 3)))
rb_c = getRB(pos=pos_c, cshape=SphereShape(1))
rb_d = getRB(pos=pos_d, cshape=BoxShape(Vec3(1, 2, 3)))
frameInA = Transform(Quaternion(0, 0, 0, 1), pos_b)
frameInB = Transform(Quaternion(0, 0, 0, 1), pos_a)
# Connect the two rigid bodies at their left/right boundary.
pivot_a, pivot_b, pivot_c, pivot_d = pos_a, pos_b, pos_c, pos_d
p2p_ab = Point2PointConstraint(rb_a, rb_b, pivot_a, pivot_b)
dof_bc = Generic6DofSpring2Constraint(rb_b, rb_c, frameInA, frameInB)
p2p_cd = Point2PointConstraint(rb_c, rb_d, pivot_c, pivot_d)
# Add both rigid bodies into a simulation.
bb = BulletBase()
bb.addRigidBody(rb_a)
bb.addRigidBody(rb_b)
# So far we have not added any constraints.
assert bb.getNumConstraints() == 0
assert bb.getConstraint(0) == None
assert bb.getConstraint(10) == None
# Add the first constraint.
bb.addConstraint(p2p_ab)
assert bb.getNumConstraints() == 1
assert bb.getConstraint(0) == p2p_ab
assert bb.getConstraint(1) == None
assert list(bb.iterateConstraints()) == [p2p_ab]
# Add the first constraint a second time. The function call must suceed
# but the constraint must not have been added again.
bb.addConstraint(p2p_ab)
assert bb.getNumConstraints() == 1
assert bb.getConstraint(0) == p2p_ab
assert bb.getConstraint(1) == None
assert list(bb.iterateConstraints()) == [p2p_ab]
# Add the second and third constraint.
bb.addConstraint(dof_bc)
assert bb.getNumConstraints() == 2
assert list(bb.iterateConstraints()) == [p2p_ab, dof_bc]
bb.addConstraint(p2p_cd)
assert bb.getNumConstraints() == 3
assert bb.getConstraint(0) == p2p_ab
assert bb.getConstraint(1) == dof_bc
assert bb.getConstraint(2) == p2p_cd
assert list(bb.iterateConstraints()) == [p2p_ab, dof_bc, p2p_cd]
# Remove the middle constraint twice.
p2p_none = Point2PointConstraint(rb_a, rb_d, pivot_b, pivot_c)
for ii in range(2):
bb.removeConstraint(dof_bc)
assert bb.getNumConstraints() == 2
assert bb.getConstraint(0) == p2p_ab
assert bb.getConstraint(1) == p2p_cd
assert bb.getConstraint(2) == None
# Remove non-existing constraint.
bb.removeConstraint(p2p_none)
def test_Generic6DofSpring2Constraint_emulateSlider_pivot_sim(self):
"""
The '6DofSpring2Constraint' (note the '2' in the name) behave slight
differently than the '6DofSpringConstraint' (no '2' in the name). For
this reason it has a dedicated test.
The differences are subtle and, as usual with Bullet, not really
documented. However, the '2' version is (apparently) more stable and I
know that the damping coefficient works (unlike in the version without
'2' where it has no effect).
One of the main difference, and also the reason why the previous slider
test would fail with the '2' version is that the slider does not have
to extend fully before the second object experiences a force. This is
rather plausible for all realistic slider constraints but hard to test
rigorously. However, this test attempts to verify this basic feature
nevertheless even though the limits are somewhat empirical.
"""
# Create two rigid bodies side by side (they *do* touch, but just).
pos_a = Vec3(-1, 0, 0)
pos_b = Vec3(1, 0, 0)
rb_a = getRB(pos=pos_a, cshape=SphereShape(1))
rb_b = getRB(pos=pos_b, cshape=BoxShape(Vec3(1, 2, 3)))
# Create the constraint between the two bodies. The constraint applies
# at (0, 0, 0) in world coordinates.
frameInA = Transform(Quaternion(0, 0, 0, 1), pos_b)
frameInB = Transform(Quaternion(0, 0, 0, 1), pos_a)
clsDof6 = Generic6DofSpring2Constraint
dof = clsDof6(rb_a, rb_b, frameInA, frameInB)
# We are now emulating a slider constraint with this 6DOF constraint.
# For this purpose we need to specify the linear/angular limits.
sliderLimitLo = -1
sliderLimitHi = -sliderLimitLo
# Apply the linear/angular limits.
dof.setLinearLowerLimit(Vec3(sliderLimitLo, 0, 0))
dof.setLinearUpperLimit(Vec3(sliderLimitHi, 0, 0))
# Add both rigid bodies and the constraint to the Bullet simulation.
bb = BulletBase()
bb.setGravity(Vec3(0, 0, 0))
bb.addRigidBody(rb_a)
bb.addRigidBody(rb_b)
bb.addConstraint(dof)
# Verify that the objects are at x-position +/-1, and thus 2 Meters
# apart.
p_a = rb_a.getCenterOfMassTransform().getOrigin().topy()
p_b = rb_b.getCenterOfMassTransform().getOrigin().topy()
init_pos = (p_a[0], p_b[0])
assert init_pos == (-1, 1)
# The ...Spring2... (notice the '2') behaves slightly differently (and
# more correctly) than the Dof without the '2' in the name. In
# particular, once the slider constraint extends to ~1/sqrt(2) of the
# full distance it starts to dampen the motion. For this purpose we
# define three regions: half extended, between half- and fully extend,
# and fully extended. The two threshold values below specify the two
# boundary regions.
thresh_1 = init_pos[1] + sliderLimitHi / 2
thresh_2 = init_pos[1] + sliderLimitHi
# Pull the right object to the right. Initially this must not affect
# the object on the left until the slider is fully extended, at which
# point the left object must begin to move as well.
for ii in range(50):
# Apply the force and step the simulation.
rb_b.applyCentralForce(Vec3(10, 0, 0))
bb.stepSimulation(1 / 60, 60)
# Query the position of the objects.
p_a = rb_a.getCenterOfMassTransform().getOrigin().topy()
p_b = rb_b.getCenterOfMassTransform().getOrigin().topy()
# If the right object has not moved far enough to extend the slider
# by at least half then the left object must remain still. If it is
# in the region between half- and full extend then the object will
# eventually start to move (but we do not know exactly when). When
# the right object has moved enough to fully extend the slider the
# left object must definitively have moved by then.
if p_b[0] < thresh_1:
assert p_a[0] == init_pos[0]
elif thresh_1 <= p_b[0] < thresh_2:
assert p_a[0] >= init_pos[0]
else:
assert p_a[0] > init_pos[0]
# Verify that the above loop really pulled the right object far enought
# to exhaust the maximum translation allowance.
assert p_b[0] > (init_pos[1] + sliderLimitHi)
