def startController(objID):
"""
Alternately apply an up/down force to itself.
This is a simplistic demo of how a controller script can influence an
object in the simulation.
"""
# Connect to Clerk.
client = azrael.client.Client()
# Specify the toggle interval in seconds.
toggle_interval = 4.0
# Fire the booster asymmetrically to get the object spinning.
cmd_0 = types.CmdBooster(partID='0', force=1.0)
cmd_1 = types.CmdBooster(partID='1', force=0.9)
client.controlParts(objID, [cmd_0, cmd_1], [])
time.sleep(0.1)
# Turn off the boosters and prevent further angular acceleration.
cmd_0 = types.CmdBooster(partID='0', force=0)
cmd_1 = types.CmdBooster(partID='1', force=0)
client.controlParts(objID, [cmd_0, cmd_1], [])
time.sleep(toggle_interval)
# Define the commands to spawn objects from factory.
cmd_2 = types.CmdFactory(partID='0', exit_speed=0.1)
cmd_3 = types.CmdFactory(partID='1', exit_speed=0.9)
# Periodically apply a boost and spawn objects.
while True:
# Turn the boosters on (symmetrically).
cmd_0 = types.CmdBooster(partID='0', force=5)
cmd_1 = types.CmdBooster(partID='1', force=5)
client.controlParts(objID, [cmd_0, cmd_1], [])
time.sleep(1)
# Turn the boosters off.
cmd_0 = types.CmdBooster(partID='0', force=0)
cmd_1 = types.CmdBooster(partID='1', force=0)
client.controlParts(objID, [cmd_0, cmd_1], [])
time.sleep(toggle_interval)
# Spawn objects.
client.controlParts(objID, [], [cmd_2, cmd_3])
time.sleep(toggle_interval)
def startController(objID):
# Connect to Clerk.
client = azrael.client.Client()
# ---------------------------------------------------------------------
# Central booster (partID='1')
# ---------------------------------------------------------------------
# time.sleep(2)
# Engage. This will accelerate the sphere forwards.
print('Fire central booster...', end='', flush=True)
central = types.CmdBooster(partID='1', force=20)
client.controlParts(objID, [central], [])
return
# Turn off after 4s.
time.sleep(2)
central = types.CmdBooster(partID='1', force=0)
# client.controlParts(objID, [central], [])
print('done')
# ---------------------------------------------------------------------
# Peripheral booster to the left and right (partID='0' and partID='2')
# ---------------------------------------------------------------------
# Engage. This will induce spinning due to the booster positions.
print('Fire peripheral boosters...', end='', flush=True)
left = types.CmdBooster(partID='0', force=10)
right = types.CmdBooster(partID='2', force=10)
client.controlParts(objID, [left, right], [])
# Turn off after 2s.
time.sleep(2)
left = types.CmdBooster(partID='0', force=0)
right = types.CmdBooster(partID='2', force=0)
client.controlParts(objID, [left, right], [])
print('done')
def main(objID, ref_pos):
"""
:param int objID: ID of sphere.
:param float ref_pos: desired position in space.
"""
# Connect to Azrael.
client = azrael.client.Client()
# Time step for polling/updating the booster values.
dt = 0.3
# Instantiate a PD controller.
PD = PDController(K_p=0.1, K_d=0.1, dt=dt)
# Periodically query the sphere's position, pass it to the controller to
# obtain the force values to moved it towards the desired position, and
# send those forces to sphere's boosters.
while True:
# Query the sphere's position.
ret = client.getRigidBodies([objID])
assert ret.ok
pos = ret.data[objID]['rbs'].position
# Call the controller with the current- and desired position.
force, err = PD.update(pos, ref_pos)
# Create the commands to apply the forces and visually update the
# "flames" coming out of the boosters.
forceCmds, fragStates = compileCommands(force)
# Send the force commands to Azrael.
assert client.controlParts(objID, forceCmds, []).ok
# Send the updated fragment- sizes and position to Azrael.
assert client.setFragmentStates({objID: fragStates}).ok
# Dump some info.
print('Pos={0:+.2f}, {1:+.2f}, {2:+.2f} '
'Err={3:+.2f}, {4:+.2f}, {5:+.2f} '
'Force={6:+.2f}, {7:+.2f}, {8:+.2f}'
.format(pos[0], pos[1], pos[2],
err[0], err[1], err[2],
force[0], force[1], force[2]))
# Wait one time step.
time.sleep(dt)
def run(self):
client = azrael.client.Client(ip=self.ip, port_clerk=self.port)
# ---------------------------------------------------------------------
# Edit here to change the force of boosters.
# ---------------------------------------------------------------------
# Turn both boosters on after 2s.
left = types.CmdBooster(self.left, force=0.1)
right = types.CmdBooster(self.right, force=0.1)
client.controlParts(self.objID, [right, left], [])
print('{0:02d}: Manoeuvre 1'.format(self.objID))
time.sleep(2)
# Fire the booster asymmetrically to make the cube turn.
left = types.CmdBooster(self.left, force=0)
right = types.CmdBooster(self.right, force=1)
client.controlParts(self.objID, [right, left], [])
print('{0:02d}: Manoeuvre 2'.format(self.objID))
time.sleep(2)
# Reverse the force settings to stop the spinning.
left = types.CmdBooster(self.left, force=1)
right = types.CmdBooster(self.right, force=0)
client.controlParts(self.objID, [right, left], [])
print('{0:02d}: Manoeuvre 3'.format(self.objID))
time.sleep(2)
# Use the same force on both boosters to just move forward without
# inducing any more spinning.
left = types.CmdBooster(self.left, force=0.1)
right = types.CmdBooster(self.right, force=0.1)
client.controlParts(self.objID, [right, left], [])
time.sleep(4)
# Done.
print('{0:02d}: Manoeuvre 4'.format(self.objID))
def test_controlParts(self, client_type):
"""
Create a template with boosters and factories. Then send control
commands to them and ensure the applied forces, torques, and
spawned objects are correct.
In this test the parent object moves and is oriented away from its
default.
"""
# Get the client for this test.
client = self.clients[client_type]
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Parameters and constants for this test.
objID_1 = 1
pos_parent = [1, 2, 3]
vel_parent = [4, 5, 6]
# Part positions relative to parent.
dir_0 = [0, 0, +2]
dir_1 = [0, 0, -1]
pos_0 = [0, 0, +3]
pos_1 = [0, 0, -4]
# Describes a rotation of 180 degrees around x-axis.
orient_parent = [1, 0, 0, 0]
# Part position in world coordinates if the parent is rotated by 180
# degrees around the x-axis. The normalisation of the direction is
# necessary because the parts will automatically normalise all
# direction vectors, including dir_0 and dir_1 which are not unit
# vectors.
dir_0_out = -np.array(dir_0) / np.sum(abs(np.array(dir_0)))
dir_1_out = -np.array(dir_1) / np.sum(abs(np.array(dir_1)))
pos_0_out = -np.array(pos_0)
pos_1_out = -np.array(pos_1)
# ---------------------------------------------------------------------
# Create a template with two factories and spawn it.
# ---------------------------------------------------------------------
# Define the parts.
boosters = {
'0': types.Booster(pos=pos_0, direction=dir_0,
minval=0, maxval=0.5, force=0),
'1': types.Booster(pos=pos_1, direction=dir_1,
minval=0, maxval=1.0, force=0)
}
factories = {
'0': types.Factory(pos=pos_0, direction=dir_0,
templateID='_templateBox',
exit_speed=[0.1, 0.5]),
'1': types.Factory(pos=pos_1, direction=dir_1,
templateID='_templateSphere',
exit_speed=[1, 5])
}
# Define the template, add it to Azrael, and spawn an instance.
temp = getTemplate('t1',
rbs=getRigidBody(),
boosters=boosters,
factories=factories)
assert client.addTemplates([temp]).ok
new_obj = {'templateID': temp.aid,
'rbs': {
'position': pos_parent,
'velocityLin': vel_parent,
'rotation': orient_parent}}
ret = client.spawn([new_obj])
assert ret.ok and (ret.data == [objID_1])
del boosters, factories, temp, new_obj
# ---------------------------------------------------------------------
# Activate booster and factories and verify that the applied force and
# torque is correct, as well as that the spawned objects have the
# correct state variables attached to them.
# ---------------------------------------------------------------------
# Create the commands to let each factory spawn an object.
exit_speed_0, exit_speed_1 = 0.2, 2
forcemag_0, forcemag_1 = 0.2, 0.4
cmd_b = {
'0': types.CmdBooster(force=forcemag_0),
'1': types.CmdBooster(force=forcemag_1),
}
cmd_f = {
'0': types.CmdFactory(exit_speed=exit_speed_0),
'1': types.CmdFactory(exit_speed=exit_speed_1),
}
# Send the commands and ascertain that the returned object IDs now
# exist in the simulation. These IDs must be '2' and '3'.
ret = client.controlParts(objID_1, cmd_b, cmd_f)
id_2, id_3 = 2, 3
assert (ret.ok, ret.data) == (True, [id_2, id_3])
# Query the state variables of the objects spawned by the factories.
ok, _, ret_SVs = client.getRigidBodies([id_2, id_3])
assert (ok, len(ret_SVs)) == (True, 2)
# Determine which body was spawned by which factory based on their
# position. We do this by looking at their initial position which
# *must* match one of the parents.
body_2, body_3 = ret_SVs[id_2]['rbs'], ret_SVs[id_3]['rbs']
if np.allclose(body_2.position, pos_1_out + pos_parent):
body_2, body_3 = body_3, body_2
# Verify the position and velocity of the spawned objects is correct.
ac = np.allclose
assert ac(body_2.velocityLin, exit_speed_0 * dir_0_out + vel_parent)
assert ac(body_2.position, pos_0_out + pos_parent)
assert ac(body_3.velocityLin, exit_speed_1 * dir_1_out + vel_parent)
assert ac(body_3.position, pos_1_out + pos_parent)
# Let Leonard sync its data and then verify it received the correct
# total force and torque exerted by the boosters.
leo.processCommandsAndSync()
forcevec_0, forcevec_1 = forcemag_0 * dir_0_out, forcemag_1 * dir_1_out
tot_force = forcevec_0 + forcevec_1
tot_torque = (np.cross(pos_0_out, forcevec_0) +
np.cross(pos_1_out, forcevec_1))
# Query the torque and force from Azrael and verify they are correct.
leo_force, leo_torque = leo.totalForceAndTorque(objID_1)
assert np.array_equal(leo_force, tot_force)
assert np.array_equal(leo_torque, tot_torque)
