def insert_table(world: scenario_gazebo.World) -> scenario_gazebo.Model:
# Insert objects from Fuel
uri = lambda org, name: f"https://fuel.ignitionrobotics.org/{org}/models/{name}"
# Download the cube SDF file
bucket_sdf = scenario_gazebo.get_model_file_from_fuel(
uri=uri(org="OpenRobotics", name="Table"), use_cache=False
)
# Assign a custom name to the model
model_name = "table"
# Insert the model
assert world.insert_model(bucket_sdf, scenario_core.Pose_identity(), model_name)
# Return the model
return world.get_model(model_name=model_name)
def get_model(gazebo: scenario.GazeboSimulator,
gym_ignition_models_name: str) -> scenario.Model:
# Get the world and cast it to a Gazebo world
world = gazebo.get_world().to_gazebo()
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
model_urdf = gym_ignition_models.get_model_file(gym_ignition_models_name)
assert world.insert_model(model_urdf, core.Pose_identity(),
gym_ignition_models_name)
# Get the model and cast it to a Gazebo model
model = world.get_model(gym_ignition_models_name).to_gazebo()
assert model.id() != 0
assert model.valid()
return model
def test_download_model_from_fuel(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
# Get the default world
world = gazebo.get_world()
# Download a model from Fuel (testing a name with spaces)
model_name = "Electrical Box"
model_sdf = scenario.get_model_file_from_fuel(
f"https://fuel.ignitionrobotics.org/openrobotics/models/{model_name}", False
)
assert model_sdf
assert world.insert_model(model_sdf, core.Pose_identity())
assert model_name in world.model_names()
# Insert another model changing its name
other_model_name = "my_box"
other_model_pose = core.Pose([3.0, 0.0, 0.0], [1.0, 0, 0, 0])
assert world.insert_model(model_sdf, other_model_pose, other_model_name)
assert other_model_name in world.model_names()
assert gazebo.run()
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)
def test_actuation_delay():
# Get the simulator and the world
gazebo, world = init_gazebo_sim(step_size=0.001,
real_time_factor=1.0,
steps_per_run=1)
# 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, scenario_core.Pose_identity(),
model_name)
# Get the model
panda = world.get_model(model_name)
assert panda.valid()
assert gazebo.run(paused=True)
# Then, insert the actuation delay plugin.
# IMPORTANT: it has to be added BEFORE switching to position control mode!
actuation_delay = plugins.ActuationDelay(delay=50)
assert panda.to_gazebo().insert_model_plugin(*actuation_delay.args())
# Set the controller period equal to the physics step (1000Hz)
panda.set_controller_period(gazebo.step_size())
# Check that we have gains for all joints
assert set(panda.joint_names()) == set(panda_pid_gains_1000Hz.keys())
# Set the PID gains
for joint_name, pid in panda_pid_gains_1000Hz.items():
assert panda.get_joint(joint_name).set_pid(pid=pid)
# Reset joint1 to its middle position
joint1 = panda.get_joint("panda_joint1").to_gazebo()
joint1_range = np.abs(joint1.position_limit().max -
joint1.position_limit().min)
joint1_middle = joint1.position_limit().min + joint1_range / 2
assert joint1.reset_position(joint1_middle)
# Control the model in position. It sets the current position as new target.
assert panda.set_joint_control_mode(
scenario_core.JointControlMode_position)
# Get the current positions
q0 = panda.joint_positions()
# Build a new reference for a single joint
q0_j1 = joint1.position()
q_j1 = (0.9 * joint1_range / 2 * np.sin(2 * np.pi * 0.33 * t)
for t in np.arange(start=0, stop=10.0, step=gazebo.step_size()))
# Create a queue
queue = Queue(maxsize=actuation_delay.delay)
for _ in range(actuation_delay.delay):
# Start applying the new target
j1_ref = q0_j1 + next(q_j1)
assert joint1.set_position_target(position=j1_ref)
# Store the references in the queue
queue.put_nowait(j1_ref)
assert gazebo.run()
# Check that the robot didn't move
assert panda.joint_positions() == pytest.approx(q0, abs=np.deg2rad(1))
for _ in range(2_000):
# Continue applying the
j1_ref = q0_j1 + next(q_j1)
assert joint1.set_position_target(position=j1_ref)
assert gazebo.run()
# Compare the current position with the delayed target (popping it)
assert joint1.position() == pytest.approx(queue.get_nowait(),
abs=np.deg2rad(1))
# Add the new target
queue.put_nowait(j1_ref)