def test_step_simulation(self):
"""
Test the @stepSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False, auto_step=False)
manager.stepSimulation(client)
with self.assertRaises(pybullet.error):
manager.stepSimulation(client + 1)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False,
use_shared_memory=True,
auto_step=False)
manager.stepSimulation(client)
with self.assertRaises(pybullet.error):
manager.stepSimulation(client + 1)
manager.stopSimulation(client)
robot = simulation_manager.spawnRomeo(client, spawn_ground_plane=True)
else:
print("You have to specify a robot, pepper, nao or romeo.")
simulation_manager.stopSimulation(client)
sys.exit(1)
time.sleep(1.0)
joint_parameters = list()
for name, joint in robot.joint_dict.items():
if "Finger" not in name and "Thumb" not in name:
joint_parameters.append(
(p.addUserDebugParameter(name, joint.getLowerLimit(),
joint.getUpperLimit(),
robot.getAnglesPosition(name)), name))
try:
while True:
for joint_parameter in joint_parameters:
robot.setAngles(joint_parameter[1],
p.readUserDebugParameter(joint_parameter[0]),
1.0)
# Step the simulation
simulation_manager.stepSimulation(client)
except KeyboardInterrupt:
pass
finally:
simulation_manager.stopSimulation(client)