def test_add_get_constraint(self):
"""
Add- and get constraints.
"""
# Define the constraints.
p2p = getP2P(rb_a='1', rb_b='2', pivot_a=(0, 1, 2), pivot_b=(3, 4, 5))
dof = get6DofSpring2(rb_a='1', rb_b='2')
# ----------------------------------------------------------------------
# Client --> Clerk.
# ----------------------------------------------------------------------
for con in (p2p, dof):
payload = {'constraints': [con._asdict()]}
# Convert to JSON and back (simulates the wire transmission).
enc = json.loads(json.dumps(payload))
# Decode the data.
dec_con = protocol.ToClerk_AddConstraints_Decode(enc)
dec_con = dec_con['constraints']
assert len(dec_con) == 1
assert dec_con[0] == con
# ----------------------------------------------------------------------
# Clerk --> Client
# ----------------------------------------------------------------------
for con in (p2p, dof):
# Encode source data and simulate wire transmission.
enc = protocol.FromClerk_GetConstraints_Encode([con])
enc = json.loads(json.dumps(enc))
# Decode the data.
assert len(enc) == 1
assert aztypes.ConstraintMeta(**enc[0]) == con
def test_constraint_p2p(self, clsLeonard):
"""
Link two bodies together with a Point2Point constraint and verify that
they move together.
"""
# Get a Leonard instance.
leo = getLeonard(clsLeonard)
# Convenience.
id_a, id_b = 1, 2
pos_a, pos_b = (-2, 0, 0), (2, 0, 0)
distance = abs(pos_a[0] - pos_b[0])
assert distance >= 4
body_a = getRigidBody(position=pos_a, cshapes={'cssphere': getCSSphere()})
body_b = getRigidBody(position=pos_b, cshapes={'cssphere': getCSSphere()})
# Specify the constraints.
con = getP2P(rb_a=id_a, rb_b=id_b, pivot_a=pos_b, pivot_b=pos_a)
self.igor.addConstraints([con])
# Spawn both objects.
assert leoAPI.addCmdSpawn([(id_a, body_a), (id_b, body_b)]).ok
leo.processCommandsAndSync()
# Apply a force to the left sphere only.
assert leoAPI.addCmdDirectForce(id_a, [-10, 0, 0], [0, 0, 0]).ok
leo.processCommandsAndSync()
# Both object must have moved the same distance 'delta' because they
# are linked. Their distance must not have changed.
leo.step(1.0, 60)
allObjs = leo.allBodies
delta_a = allObjs[id_a].position - np.array(pos_a)
delta_b = allObjs[id_b].position - np.array(pos_b)
assert delta_a[0] < pos_a[0]
assert np.allclose(delta_a, delta_b)
tmp = abs(allObjs[id_a].position[0] - allObjs[id_b].position[0])
assert abs(tmp - distance) < 0.01
del tmp
# Unlink the objects again, apply a right-pointing force to the
# right object and verify that the left continues to move left and the
# right does not.
assert self.igor.deleteConstraints([con]) == (True, None, 1)
assert leoAPI.addCmdDirectForce(id_b, [10, 0, 0], [0, 0, 0]).ok
leo.processCommandsAndSync()
leo.step(1.0, 60)
# The distance between the spheres must have increases since they are
# not linked anymore.
tmp = abs(allObjs[id_a].position[0] - allObjs[id_b].position[0])
assert tmp > (distance + 1)
def test_add_get_remove_constraints(self, client_type):
"""
Create some bodies. Then add/query/remove constraints.
This test only verifies that the Igor interface works. It does *not*
verify that the objects are really linked in the actual simulation.
"""
# Reset the constraint database.
igor = azrael.igor.Igor()
assert igor.reset().ok
# Get the client for this test.
client = self.clients[client_type]
# Spawn the two bodies.
pos_1, pos_2, pos_3 = [-2, 0, 0], [2, 0, 0], [6, 0, 0]
new_objs = [
{'templateID': '_templateSphere', 'rbs': {'position': pos_1}},
{'templateID': '_templateSphere', 'rbs': {'position': pos_2}},
{'templateID': '_templateSphere', 'rbs': {'position': pos_3}}
]
id_1, id_2, id_3 = 1, 2, 3
assert client.spawn(new_objs) == (True, None, [id_1, id_2, id_3])
# Define the constraints.
con_1 = getP2P(rb_a=id_1, rb_b=id_2, pivot_a=pos_2, pivot_b=pos_1)
con_2 = get6DofSpring2(rb_a=id_2, rb_b=id_3)
# Verify that no constraints are currently active.
assert client.getConstraints(None) == (True, None, [])
assert client.getConstraints([id_1]) == (True, None, [])
# Add both constraints and verify they are returned correctly.
assert client.addConstraints([con_1, con_2]) == (True, None, 2)
ret = client.getConstraints(None)
assert ret.ok and (sorted(ret.data) == sorted([con_1, con_2]))
ret = client.getConstraints([id_2])
assert ret.ok and (sorted(ret.data) == sorted([con_1, con_2]))
assert client.getConstraints([id_1]) == (True, None, [con_1])
assert client.getConstraints([id_3]) == (True, None, [con_2])
# Remove the second constraint and verify the remaining constraint is
# returned correctly.
assert client.deleteConstraints([con_2]) == (True, None, 1)
assert client.getConstraints(None) == (True, None, [con_1])
assert client.getConstraints([id_1]) == (True, None, [con_1])
assert client.getConstraints([id_2]) == (True, None, [con_1])
assert client.getConstraints([id_3]) == (True, None, [])
def test_ConstraintMeta(self):
for Getter in (getP2P, get6DofSpring2):
con_a = Getter()
con_b = Getter()
assert con_a == con_b
assert self.isJsonCompatible(con_a, ConstraintMeta)
# Verify that 'FragMeta._asdict' also converts the 'fragdata' field
# to dictionaries.
con_t = getP2P()
con_d = con_t._asdict()
assert isinstance(con_d, dict)
tmp = con_t.condata._asdict()
assert isinstance(tmp, dict)
assert tmp == con_d["condata"]
def test_ConstraintMeta(self):
for Getter in (getP2P, get6DofSpring2):
con_a = Getter()
con_b = Getter()
assert con_a == con_b
assert self.isJsonCompatible(con_a, ConstraintMeta)
# Verify that 'FragMeta._asdict' also converts the 'fragdata' field
# to dictionaries.
con_t = getP2P()
con_d = con_t._asdict()
assert isinstance(con_d, dict)
tmp = con_t.condata._asdict()
assert isinstance(tmp, dict)
assert tmp == con_d['condata']
def test_specify_constraints_invalid(self):
"""
Call the constraint- related methods with invalid data and verify that
nothing breaks.
"""
# Instantiate Bullet engine.
sim = azrael.bullet_api.PyBulletDynamicsWorld(1)
# Create to spheres.
id_a, id_b = 10, 20
obj_a = getRigidBody(cshapes={'cssphere': getCSSphere()})
obj_b = getRigidBody(cshapes={'cssphere': getCSSphere()})
# An empty list is valid, albeit nothing will happen.
assert sim.setConstraints([]).ok
# Invalid constraint types.
assert not sim.setConstraints([1]).ok
# Compile the constraint.
pivot_a, pivot_b = (0, 0, 0), (1, 1, 1)
con = [getP2P(rb_a=id_a, rb_b=id_b, pivot_a=pivot_a, pivot_b=pivot_b)]
# Constraint is valid but the objects do not exist.
assert not sim.setConstraints([con]).ok
# Add one sphere to the world.
sim.setRigidBodyData(id_a, obj_a)
# Load the constraints into the physics engine.
assert not sim.setConstraints(con).ok
# Load the second sphere and apply the constraint. This time it must
# have worked.
sim.setRigidBodyData(id_b, obj_b)
assert sim.setConstraints(con).ok
# Clear all constraints
assert sim.clearAllConstraints().ok
def test_specify_constraints_invalid(self):
"""
Call the constraint- related methods with invalid data and verify that
nothing breaks.
"""
# Instantiate Bullet engine.
sim = azrael.bullet_api.PyBulletDynamicsWorld(1)
# Create to spheres.
id_a, id_b = '10', '20'
obj_a = getRigidBody(cshapes={'cssphere': getCSSphere()})
obj_b = getRigidBody(cshapes={'cssphere': getCSSphere()})
# An empty list is valid, albeit nothing will happen.
assert sim.setConstraints([]).ok
# Invalid constraint types.
assert not sim.setConstraints([1]).ok
# Compile the constraint.
pivot_a, pivot_b = (0, 0, 0), (1, 1, 1)
con = [getP2P(rb_a=id_a, rb_b=id_b, pivot_a=pivot_a, pivot_b=pivot_b)]
# Constraint is valid but the objects do not exist.
assert not sim.setConstraints([con]).ok
# Add one sphere to the world.
sim.setRigidBodyData(id_a, obj_a)
# Load the constraints into the physics engine.
assert not sim.setConstraints(con).ok
# Load the second sphere and apply the constraint. This time it must
# have worked.
sim.setRigidBodyData(id_b, obj_b)
assert sim.setConstraints(con).ok
# Clear all constraints
assert sim.clearAllConstraints().ok
def test_specify_P2P_constraint(self):
"""
Use a P2P constraint to test the various methods to add- and remove
constraints.
"""
# Instantiate Bullet engine.
sim = azrael.bullet_api.PyBulletDynamicsWorld(1)
# Create identical unit spheres at x=+/-1.
id_a, id_b = 10, 20
pos_a = (-1, 0, 0)
pos_b = (1, 0, 0)
obj_a = getRigidBody(position=pos_a, cshapes={'cssphere': getCSSphere()})
obj_b = getRigidBody(position=pos_b, cshapes={'cssphere': getCSSphere()})
# Load the objects into the physics engine.
sim.setRigidBodyData(id_a, obj_a)
sim.setRigidBodyData(id_b, obj_b)
# Compile the constraint.
pivot_a, pivot_b = pos_b, pos_a
con = [getP2P(rb_a=id_a, rb_b=id_b, pivot_a=pivot_a, pivot_b=pivot_b)]
# Load the constraints into the physics engine.
assert sim.setConstraints(con).ok
# Step the simulation. Nothing must happen.
sim.compute([id_a, id_b], 1.0, 60)
ret_a = sim.getRigidBodyData(id_a)
ret_b = sim.getRigidBodyData(id_b)
assert ret_a.ok and ret_b.ok
assert np.allclose(ret_a.data.position, pos_a)
assert np.allclose(ret_b.data.position, pos_b)
# Apply a force that will pull the left object further to the left.
sim.applyForceAndTorque(id_a, (-10, 0, 0), (0, 0, 0))
# Step the simulation. Both objects must have moved (almost) exactly
# the same amount "delta".
sim.compute([id_a, id_b], 1.0, 60)
ret_a = sim.getRigidBodyData(id_a)
ret_b = sim.getRigidBodyData(id_b)
assert ret_a.ok and ret_b.ok
delta_a = np.array(ret_a.data.position) - np.array(pos_a)
delta_b = np.array(ret_b.data.position) - np.array(pos_b)
assert np.allclose(delta_a, delta_b)
assert delta_a[1] == delta_a[2] == 0
# Remove all constraints (do it twice to test the case when there are
# no constraints).
assert sim.clearAllConstraints().ok
assert sim.clearAllConstraints().ok
# Overwrite the objects with the default data (ie put them back into
# the original position and set their velocity to zero).
sim.setRigidBodyData(id_a, obj_a)
sim.setRigidBodyData(id_b, obj_b)
sim.compute([id_a, id_b], 1.0, 60)
ret_a = sim.getRigidBodyData(id_a)
ret_b = sim.getRigidBodyData(id_b)
assert ret_a.ok and ret_b.ok
assert np.allclose(ret_a.data.position, pos_a)
assert np.allclose(ret_b.data.position, pos_b)
# Apply a force that will pull the left object further to the left.
# However, now *only* the left one must move because there are not
# constraint anymore.
sim.applyForceAndTorque(id_a, (-10, 0, 0), (0, 0, 0))
sim.compute([id_a, id_b], 1.0, 60)
ret_a = sim.getRigidBodyData(id_a)
ret_b = sim.getRigidBodyData(id_b)
assert ret_a.ok and ret_b.ok
assert not np.allclose(ret_a.data.position, pos_a)
assert np.allclose(ret_b.data.position, pos_b)
def test_mergeConstraintSets(self):
"""
Create a few disjoint sets, specify some constraints, and verify that
they are merged correctly.
"""
def _verify(_coll_sets, _correct_answer):
"""
Assert that the ``_coll_sets`` are reduced to ``_correct_answer``
by the `mergeConstraintSets` algorithm.
"""
# Fetch all unique object pairs connected by a constraint.
assert self.igor.updateLocalCache().ok
ret = self.igor.uniquePairs()
assert ret.ok
# Merge all the sets `_coll_sets` that are connected by at least
# one constraint.
ret = azrael.leonard.mergeConstraintSets(ret.data, _coll_sets)
assert ret.ok
computed = ret.data
# Compare the computed- with the expected output.
computed = [sorted(tuple(_)) for _ in computed]
correct = [sorted(tuple(_)) for _ in _correct_answer]
assert sorted(computed) == sorted(correct)
# Convenience.
igor = self.igor
mergeConstraintSets = azrael.leonard.mergeConstraintSets
# Empty set.
self.igor.reset()
assert self.igor.updateLocalCache().ok
ret = self.igor.uniquePairs()
assert ret.ok
assert mergeConstraintSets(ret.data, []) == (True, None, [])
_verify([], [])
# Set with only one subset.
self.igor.reset()
assert self.igor.updateLocalCache().ok
ret = self.igor.uniquePairs()
assert ret.ok
assert mergeConstraintSets(ret.data, [[1]]) == (True, None, [[1]])
tmp = [[1, 2, 3]]
assert mergeConstraintSets(ret.data, tmp) == (True, None, tmp)
del tmp
# Two disjoint sets.
self.igor.reset()
s = [[1], [2]]
_verify(s, s)
assert igor.addConstraints([getP2P()]).ok
_verify(s, [[1, 2]])
self.igor.reset()
_verify(s, s)
# Two disjoint sets but the constraint does not link them.
self.igor.reset()
s = [[1], [2]]
_verify(s, s)
assert igor.addConstraints([getP2P(rb_a=1, rb_b=3)]).ok
_verify(s, s)
# Three disjoint sets and the constraint links two of them.
self.igor.reset()
s = [[1, 2, 3], [4, 5], [6]]
_verify(s, s)
assert igor.addConstraints([getP2P(rb_a=1, rb_b=6)]).ok
_verify(s, [[1, 2, 3, 6], [4, 5]])
# Three disjoint sets and two constraint link both of them.
self.igor.reset()
s = [[1, 2, 3], [4, 5], [6]]
_verify(s, s)
assert igor.addConstraints([getP2P(rb_a=1, rb_b=6)]).ok
assert igor.addConstraints([getP2P(rb_a=3, rb_b=4)]).ok
_verify(s, [[1, 2, 3, 6, 4, 5]])
def test_create_constraints_with_physics(self, client_type):
"""
Spawn two rigid bodies and define a Point2Point constraint among them.
Then apply a force onto one of them and verify the second one moves
accordingly.
"""
# Reset the constraint database.
igor = azrael.igor.Igor()
assert igor.reset().ok
# Get the client for this test.
client = self.clients[client_type]
# Reset the database and instantiate a Leonard.
leo = getLeonard(azrael.leonard.LeonardBullet)
# Spawn two bodies.
pos_a, pos_b = [-2, 0, 0], [2, 0, 0]
new_objs = [
{'templateID': '_templateSphere',
'rbs': {'position': pos_a}},
{'templateID': '_templateSphere',
'rbs': {'position': pos_b}},
]
id_1, id_2 = 1, 2
assert client.spawn(new_objs) == (True, None, [id_1, id_2])
# Verify the position of the bodies.
ret = client.getObjectStates([id_1, id_2])
assert ret.ok
assert ret.data[id_1]['rbs']['position'] == pos_a
assert ret.data[id_2]['rbs']['position'] == pos_b
# Define- and add the constraints.
con = [getP2P(rb_a=id_1, rb_b=id_2, pivot_a=pos_b, pivot_b=pos_a)]
assert client.addConstraints(con) == (True, None, 1)
# Apply a force that will pull the left object further to the left.
# However, both objects must move the same distance in the same
# direction because they are now linked together.
assert client.setForce(id_1, [-10, 0, 0]).ok
leo.processCommandsAndSync()
leo.step(1.0, 60)
# Query the object positions. Due to some database timings is sometimes
# happen that the objects appear to not have moved. In that case retry
# the query a few times before moving to the comparison.
for ii in range(10):
assert ii < 9
# Query the objects and put their positions into convenience
# variables.
ret = client.getRigidBodies([id_1, id_2])
pos_a2 = ret.data[id_1]['rbs'].position
pos_b2 = ret.data[id_2]['rbs'].position
# Exit this loop if both objects have moved.
if (pos_a != pos_a2) and (pos_b != pos_b2):
break
time.sleep(0.1)
# Verify that the objects have moved to the left and maintained their
# distance.
delta_a = np.array(pos_a2) - np.array(pos_a)
delta_b = np.array(pos_b2) - np.array(pos_b)
assert delta_a[0] < pos_a[0]
assert np.allclose(delta_a, delta_b)
def test_specify_P2P_constraint(self):
"""
Use a P2P constraint to test the various methods to add- and remove
constraints.
"""
# Instantiate Bullet engine.
sim = azrael.bullet_api.PyBulletDynamicsWorld(1)
# Create identical unit spheres at x=+/-1.
id_a, id_b = '10', '20'
pos_a = (-1, 0, 0)
pos_b = (1, 0, 0)
obj_a = getRigidBody(position=pos_a,
cshapes={'cssphere': getCSSphere()})
obj_b = getRigidBody(position=pos_b,
cshapes={'cssphere': getCSSphere()})
# Load the objects into the physics engine.
sim.setRigidBodyData(id_a, obj_a)
sim.setRigidBodyData(id_b, obj_b)
# Compile the constraint.
pivot_a, pivot_b = pos_b, pos_a
con = [getP2P(rb_a=id_a, rb_b=id_b, pivot_a=pivot_a, pivot_b=pivot_b)]
# Load the constraints into the physics engine.
assert sim.setConstraints(con).ok
# Step the simulation. Both objects must stay put.
sim.compute([id_a, id_b], 1.0, 60)
ret_a = sim.getRigidBodyData(id_a)
ret_b = sim.getRigidBodyData(id_b)
assert ret_a.ok and ret_b.ok
assert np.allclose(ret_a.data.position, pos_a)
assert np.allclose(ret_b.data.position, pos_b)
# Apply a force that will pull the left object further to the left.
sim.applyForceAndTorque(id_a, (-10, 0, 0), (0, 0, 0))
# Step the simulation. Both objects must have moved (almost) exactly
# the same amount "delta".
sim.compute([id_a, id_b], 1.0, 60)
ret_a = sim.getRigidBodyData(id_a)
ret_b = sim.getRigidBodyData(id_b)
assert ret_a.ok and ret_b.ok
delta_a = np.array(ret_a.data.position) - np.array(pos_a)
delta_b = np.array(ret_b.data.position) - np.array(pos_b)
assert np.allclose(delta_a, delta_b)
assert delta_a[1] == delta_a[2] == 0
# Remove all constraints (do it twice to test the case when there are
# no constraints).
assert sim.clearAllConstraints().ok
assert sim.clearAllConstraints().ok
# Overwrite the objects with the default data (ie put them back into
# the original position and set their velocity to zero).
sim.setRigidBodyData(id_a, obj_a)
sim.setRigidBodyData(id_b, obj_b)
sim.compute([id_a, id_b], 1.0, 60)
ret_a = sim.getRigidBodyData(id_a)
ret_b = sim.getRigidBodyData(id_b)
assert ret_a.ok and ret_b.ok
assert np.allclose(ret_a.data.position, pos_a)
assert np.allclose(ret_b.data.position, pos_b)
# Apply a force that will pull the left object further to the left.
# However, now *only* the left one must move because there are not
# constraint anymore.
sim.applyForceAndTorque(id_a, (-10, 0, 0), (0, 0, 0))
sim.compute([id_a, id_b], 1.0, 60)
ret_a = sim.getRigidBodyData(id_a)
ret_b = sim.getRigidBodyData(id_b)
assert ret_a.ok and ret_b.ok
assert not np.allclose(ret_a.data.position, pos_a)
assert np.allclose(ret_b.data.position, pos_b)
