def test_initialization(gazebo: scenario.GazeboSimulator):
ok = gazebo.initialize()
rtf = gazebo.real_time_factor()
step_size = gazebo.step_size()
iterations = gazebo.steps_per_run()
if step_size <= 0:
assert not ok
assert not gazebo.initialized()
assert not gazebo.run()
if rtf <= 0:
assert not ok
assert not gazebo.initialized()
assert not gazebo.run()
if iterations <= 0:
assert not ok
assert not gazebo.initialized()
assert not gazebo.run()
if rtf > 0 and iterations > 0 and step_size > 0:
assert ok
assert gazebo.initialized()
def test_world_physics_plugin(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
world = gazebo.get_world()
dt = gazebo.step_size()
assert world.time() == 0
# Insert a cube
cube_urdf = utils.get_cube_urdf()
cube_name = "my_cube"
cube_pose = core.Pose([0, 0, 1.0], [1, 0, 0, 0])
assert world.insert_model(cube_urdf, cube_pose, cube_name)
assert cube_name in world.model_names()
cube = world.get_model(cube_name)
# There's no physics, the cube should not move
for _ in range(10):
gazebo.run()
assert cube.base_position() == cube_pose.position
assert world.time() == 0
# Insert the Physics system
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# After the first step, the physics catches up with time
gazebo.run()
assert world.time() == pytest.approx(11 * dt)
# The cube should start falling
assert cube.base_position()[2] < cube_pose.position[2]
gazebo.run()
assert world.time() == pytest.approx(12 * dt)
# Paused steps do not increase the time
gazebo.run(paused=True)
assert world.time() == pytest.approx(12 * dt)
def test_sim_time_starts_from_zero(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
world = gazebo.get_world()
dt = gazebo.step_size()
assert world.time() == 0
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
assert world.time() == 0
gazebo.run(paused=True)
assert world.time() == 0
gazebo.run(paused=False)
assert world.time() == dt
gazebo.run(paused=False)
assert world.time() == 2 * dt
gazebo.run(paused=False)
assert world.time() == pytest.approx(3 * dt)
def test_computed_torque_fixed_base(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
step_size = gazebo.step_size()
# Get the default world
world = gazebo.get_world()
# Insert the physics
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# Get the panda urdf
panda_urdf = gym_ignition_models.get_model_file("panda")
# Insert the panda arm
model_name = "panda"
assert world.insert_model(panda_urdf, core.Pose_identity(), model_name)
# import time
# gazebo.gui()
# time.sleep(3)
# gazebo.run(paused=True)
# Get the model
panda = world.get_model(model_name).to_gazebo()
# Set the controller period
panda.set_controller_period(step_size)
# Insert the controller
assert panda.insert_model_plugin(*controllers.ComputedTorqueFixedBase(
kp=[10.0] * panda.dofs(),
ki=[0.0] * panda.dofs(),
kd=[3.0] * panda.dofs(),
urdf=panda_urdf,
joints=panda.joint_names(),
).args())
# Set the references
assert panda.set_joint_position_targets([0.0] * panda.dofs())
assert panda.set_joint_velocity_targets([0.0] * panda.dofs())
assert panda.set_joint_acceleration_targets([0.0] * panda.dofs())
joints_no_fingers = [
j for j in panda.joint_names() if j.startswith("panda_joint")
]
nr_of_joints = len(joints_no_fingers)
assert nr_of_joints > 0
# Reset the joints state
q0 = [np.deg2rad(45)] * nr_of_joints
dq0 = [0.1] * nr_of_joints
assert panda.reset_joint_positions(q0, joints_no_fingers)
assert panda.reset_joint_velocities(dq0, joints_no_fingers)
assert gazebo.run(True)
assert panda.joint_positions(joints_no_fingers) == pytest.approx(q0)
assert panda.joint_velocities(joints_no_fingers) == pytest.approx(dq0)
# Step the simulator for a couple of seconds
for _ in range(3000):
gazebo.run()
# Check that the the references have been reached
assert panda.joint_positions() == pytest.approx(
panda.joint_position_targets(), abs=np.deg2rad(1))
assert panda.joint_velocities() == pytest.approx(
panda.joint_velocity_targets(), abs=0.05)
# Apply an external force
assert (panda.get_link("panda_link4").to_gazebo().apply_world_force(
[100.0, 0, 0], 0.5))
for _ in range(4000):
assert gazebo.run()
# Check that the the references have been reached
assert panda.joint_positions() == pytest.approx(
panda.joint_position_targets(), abs=np.deg2rad(1))
assert panda.joint_velocities() == pytest.approx(
panda.joint_velocity_targets(), abs=0.05)