def test_motionstate(self):
"""
Set and get a motion state.
"""
# Get RigidBody object.
body = getRB()
# Create a new Transform.
pos = Vec3(1, 2, 3.5)
rot = Quaternion(0, 1, 0, 0)
t = Transform()
t.setOrigin(pos)
t.setRotation(rot)
# It must be impossible to instantiate 'MotionState' directly.
with pytest.raises(NotImplementedError):
MotionState()
# Create a MotionState and apply the new transform.
ms_ref = DefaultMotionState()
ms_ref.setWorldTransform(t)
# Verify the MotionState does not yet match ours.
ms = body.getMotionState()
assert ms.getWorldTransform().getOrigin() != pos
assert ms.getWorldTransform().getRotation() != rot
# Apply- and query the MotionState.
body.setMotionState(ms_ref)
ms = body.getMotionState()
# Verify the MotionState is correct.
assert ms.getWorldTransform().getOrigin() == pos
assert ms.getWorldTransform().getRotation() == rot
def test_ConstructionInfo_to_RigidBody(self):
"""
Verify that the initial motion state is transferred correctly to the
RigidBody.
"""
mass = 10
pos = Vec3(1, 2, 3)
rot = Quaternion(0, 1, 0, 0)
t = Transform(rot, pos)
ms = DefaultMotionState(t)
cs = EmptyShape()
inert = Vec3(1, 2, 4)
# Compile the Rigid Body parameters.
ci = RigidBodyConstructionInfo(mass, ms, cs, Vec3(2, 4, 6))
assert ci.localInertia == Vec3(2, 4, 6)
ci.localInertia = inert
assert ci.localInertia == inert
# Construct the rigid body and delete the construction info.
body = RigidBody(ci)
del ci
# Verify that the object is at the correct position and has the correct
# mass and inertia.
t = body.getMotionState().getWorldTransform()
assert t.getOrigin() == pos
assert t.getRotation() == rot
assert body.getInvMass() == 1 / mass
assert body.getInvInertiaDiagLocal() == Vec3(1, 0.5, 0.25)
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 getRB(pos=Vec3(0, 0, 0), cshape=SphereShape(1)):
"""
Return a Rigid Body plus auxiliary information (do *not* delete; see
note below).
.. note:: Do not delete the tuple until the end of the test
because it may lead to memory access violations. The reason is that a
rigid body requires several indepenent structures that need to remain
in memory.
"""
t = Transform(Quaternion(0, 0, 0, 1), pos)
ms = DefaultMotionState(t)
mass = 1
# Build construction info and instantiate the rigid body.
ci = RigidBodyConstructionInfo(mass, ms, cshape)
return RigidBody(ci)
def test_centerOfMassTransform(self):
"""
Get/set the centerOfMassTransform.
"""
# Get RigidBody object.
body = getRB()
# Create a new Transform.
pos = Vec3(1, 2, 3.5)
rot = Quaternion(0, 1, 0, 0)
t1 = Transform()
t1.setOrigin(pos)
t1.setRotation(rot)
# Apply the Transform to the body.
body.setCenterOfMassTransform(t1)
t2 = body.getCenterOfMassTransform()
# Verify the result.
assert t1.getOrigin() == t2.getOrigin()
assert t1.getRotation() == t2.getRotation()
def test_Generic6DofConstraint(self, clsDof6):
"""
Create-, set, and query various `Generic6DofConstraint` attributes.
"""
# Create two rigid bodies side by side (they *do* touch, but just).
cs_a = SphereShape(1)
cs_b = BoxShape(Vec3(1, 2, 3))
pos_a = Vec3(-1, 0, 0)
pos_b = Vec3(1, 0, 0)
rb_a = getRB(pos=pos_a, cshape=cs_a)
rb_b = getRB(pos=pos_b, cshape=cs_b)
# Create the 6DOF constraint between the two bodies.
frameInA = Transform()
frameInB = Transform()
frameInA.setIdentity()
frameInB.setIdentity()
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 = Vec3(-10, 0, 0)
sliderLimitHi = Vec3(10, 0, 0)
angularLimitLo = Vec3(-1.5, 0, 0)
angularLimitHi = Vec3(1.5, 0, 0)
# Apply the linear/angular limits.
dof.setLinearLowerLimit(sliderLimitLo)
dof.setLinearUpperLimit(sliderLimitHi)
dof.setAngularLowerLimit(angularLimitLo)
dof.setAngularUpperLimit(angularLimitHi)
# Verify the linear/angular limits were applied correctly.
assert dof.getLinearLowerLimit() == sliderLimitLo
assert dof.getLinearUpperLimit() == sliderLimitHi
assert dof.getAngularLowerLimit() == angularLimitLo
assert dof.getAngularUpperLimit() == angularLimitHi
# Query the object type; not sure what this is, but if it does not
# segfault it works :)
dof.getObjectType()
def main():
# Create two rigid bodies side by side (they *do* touch, but just).
pos_a = Vec3(-10, 0, 0)
pos_b = Vec3(10, 0, 0)
r = 0.001
rb_a = getRB(pos=pos_a, cshape=SphereShape(r))
rb_b = getRB(pos=pos_b, cshape=SphereShape(r))
del r
# Create the constraint between the two bodies. The constraint applies
# at (0, 0, 0) in world coordinates.
frameInA = Transform(Quaternion(0, 1, 0, 0), Vec3(5, 0, 0))
frameInB = Transform(Quaternion(0, -1, 0, 0), Vec3(-5, 0, 0))
frameInA.setIdentity()
frameInB.setIdentity()
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 = -100
sliderLimitHi = 100
# Apply the linear/angular limits.
dof.setLinearLowerLimit(Vec3(sliderLimitLo, sliderLimitLo, sliderLimitLo))
dof.setLinearLowerLimit(Vec3(sliderLimitHi, sliderLimitHi, sliderLimitHi))
tmp = 0 * np.pi / 12
dof.setAngularLowerLimit(Vec3(-tmp, -tmp, -tmp))
dof.setAngularUpperLimit(Vec3(tmp, tmp, tmp))
# Activate the spring for all three translational axis and disable it
# for the three angular axis.
for ii in range(3):
dof.enableSpring(ii, True)
dof.enableSpring(3 + ii, False)
dof.setStiffness(ii, 0.1)
dof.setDamping(ii, 1)
dof.setEquilibriumPoint(ii, 0)
# Add both rigid bodies and the constraint to the Bullet simulation.
bb = azBullet.BulletBase()
bb.setGravity(Vec3(0, 0, 0))
bb.addRigidBody(rb_a)
bb.addRigidBody(rb_b)
rb_a.forceActivationState(4)
rb_b.forceActivationState(4)
bb.addConstraint(dof)
# 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.
# rb_a.applyCentralForce(Vec3(0, -1, 0))
# rb_b.applyCentralForce(Vec3(0, 1, 0))
numIter = 1000
out_a = np.zeros((numIter, 3))
out_b = np.zeros_like(out_a)
for ii in range(numIter):
if ii == 1:
rb_a.applyCentralForce(Vec3(0, 0, 0))
rb_b.applyCentralForce(Vec3(0, 0, 0))
# Step simulation.
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()
out_a[ii, :] = p_a
out_b[ii, :] = p_b
animateMotion(out_a, out_b)
def test_transform(self):
"""
Test the Transform class.
"""
pos = Vec3(1, 2, 3.5)
rot = Quaternion(0, 1, 0, 0)
t = Transform(rot, pos)
assert t.getOrigin() == pos
assert t.getRotation() == rot
# Set to identity.
t.setIdentity()
assert t.getOrigin() == Vec3(0, 0, 0)
assert t.getRotation() == Quaternion(0, 0, 0, 1)
# Set the position and rotation.
pos = Vec3(1, 2, 3.5)
rot = Quaternion(0, 1, 0, 0)
t.setOrigin(pos)
t.setRotation(rot)
assert t.getOrigin() == pos
assert t.getRotation() == rot
# Repeat with different values.
pos = Vec3(-1, 2.5, 3.5)
rot = Quaternion(0, 0, 0, 1)
t.setOrigin(pos)
t.setRotation(rot)
assert t.getOrigin() == pos
assert t.getRotation() == rot
def moveBody(rb, pos):
trans = Transform()
trans.setOrigin(pos)
ms = DefaultMotionState()
ms.setWorldTransform(trans)
rb.setMotionState(ms)
# Instantiate the Bullet simulation.
sim = azBullet.BulletBase()
sim.setGravity(Vec3(0, 0, 0))
# Create the spherical collision shape and a compound shape.
csSphere = SphereShape(radius=1)
cs = azBullet.CompoundShape()
# Specify the mass and let Bullet compute the Inertia.
total_mass = 1
print('Inertia sphere: ', csSphere.calculateLocalInertia(total_mass))
# Add collision shapes to the compound. Its constituents are two spheres
# at different positions.
t1 = Transform(Quaternion(0, 0, 0, 1), Vec3(0, -5, 0))
t2 = Transform(Quaternion(0, 0, 0, 1), Vec3(0, 5, 0))
cs.addChildShape(t1, csSphere)
cs.addChildShape(t2, csSphere)
# The local inertia is only a crude approximation based on the AABBs. Do not
# use it. Use calculatePrincipalAxisTransform instead (see below).
print('Local Inertia AABBs: ', cs.calculateLocalInertia(total_mass))
# Assign each child 1/N of the total mass.
N = cs.getNumChildShapes()
masses = [total_mass / N for _ in range(N)]
del total_mass, N
# Compute the inertia vector (ie diagonal entries of inertia Tensor) and the
# principal axis of the Inertia tensor.
from azBullet import DefaultMotionState, Transform
from azBullet import RigidBody, RigidBodyConstructionInfo
# Instantiate the Bullet simulation.
sim = azBullet.BulletBase()
sim.setGravity(Vec3(0, -10, 0))
# Create the collision shape for Ball and Ground.
ballShape = SphereShape(1)
groundShape = StaticPlaneShape(Vec3(0, 1, 0), 1)
# Create the rigid body for the Ground. Since it is a static body its mass must
# be zero.
mass = 0
groundRigidBodyState = DefaultMotionState(
Transform(Quaternion(0, 0, 0, 1), Vec3(0, -1, 0)))
ci = RigidBodyConstructionInfo(mass, groundRigidBodyState, groundShape)
groundRigidBody = RigidBody(ci)
groundRigidBody.setRestitution(1.0)
sim.addRigidBody(groundRigidBody)
del mass, ci
# Create the rigid body for the Ball. Since it is a dynamic body we need to
# specify mass and inertia. The former we need to do ourselves but Bullet can
# do the heavy lifting for the Inertia and compute it for us.
fallRigidBodyState = DefaultMotionState(
Transform(Quaternion(0, 0, 0, 1), Vec3(0, 20, 0)))
mass = 1
fallInertia = ballShape.calculateLocalInertia(mass)
ci = RigidBodyConstructionInfo(mass, fallRigidBodyState, ballShape,
fallInertia)
