def template_compare_environments(env_name_a: str, env_name_b: str,
max_error: float):
# Create the pendulum
env_a = gym.make(env_name_a)
env_b = gym.make(env_name_b)
assert env_a.unwrapped.spec._kwargs['agent_rate'] == \
env_b.unwrapped.spec._kwargs['agent_rate']
logger.set_level(gym.logger.DEBUG)
# Render the environment
# env.render('human')
# time.sleep(5)
# Seed the environment
env_a.seed(42)
env_b.seed(42)
range_obs = env_a.observation_space.high - env_a.observation_space.low
for epoch in range(10):
# Reset the environments
observation_a = env_a.reset()
observation_b = env_b.reset()
assert np.allclose(observation_a, observation_b), \
"Observations after reset don't match"
# Initialize intermediate variables
iteration = 0
done_a = False
while not done_a:
iteration += 1
# Sample a random action from the environment a
action = env_a.action_space.sample()
# Step the environments
observation_a, _, done_a, _ = env_b.step(action)
observation_b, _, done_b, _ = env_a.step(action)
error = np.sum(
np.abs(observation_a / range_obs - observation_b / range_obs))
if error > max_error:
print("===================")
print(f"Environment A name: {env_name_a}")
print(f"Environment B name: {env_name_b}")
print(f"Rollout: #{epoch}@{iteration}")
print(f"Error: {error}")
print(f"Max Error: {max_error}")
print(f"Observation A: {observation_a}")
print(f"Observation B: {observation_b}")
print("===================")
assert False, "The error is bigger than the threshold"
env_a.close()
env_b.close()
def gazebo(self) -> bindings.GazeboWrapper:
if self._gazebo_wrapper:
assert self._gazebo_wrapper.getPhysicsData().rtf == self._rtf, \
"The RTF of gazebo does not match the configuration"
assert 1 / self._gazebo_wrapper.getPhysicsData().maxStepSize == \
self._physics_rate, \
"The physics rate of gazebo does not match the configuration"
return self._gazebo_wrapper
# =================
# INITIALIZE GAZEBO
# =================
assert self._rtf, "Real Time Factor was not set"
assert self.agent_rate, "Agent rate was not set"
assert self._physics_rate, "Physics rate was not set"
logger.debug("Starting gazebo")
logger.debug(f"Agent rate: {self.agent_rate} Hz")
logger.debug(f"Physics rate: {self._physics_rate} Hz")
logger.debug(f"Simulation RTF: {self._rtf}")
# Compute the number of physics iteration to execute at every environment step
num_of_iterations_per_gazebo_step = self._physics_rate / self.agent_rate
if num_of_iterations_per_gazebo_step != int(
num_of_iterations_per_gazebo_step):
logger.warn(
"Rounding the number of iterations to {} from the nominal {}".
format(int(num_of_iterations_per_gazebo_step),
num_of_iterations_per_gazebo_step))
else:
logger.debug("Setting {} iteration per simulator step".format(
int(num_of_iterations_per_gazebo_step)))
# Create the GazeboWrapper object
self._gazebo_wrapper = bindings.GazeboWrapper(
int(num_of_iterations_per_gazebo_step), self._rtf,
self._physics_rate)
# Set the verbosity
logger.set_level(gym.logger.MIN_LEVEL)
# Initialize the world
world_ok = self._gazebo_wrapper.setupGazeboWorld(self._world)
assert world_ok, "Failed to initialize the gazebo world"
# Initialize the ignition gazebo wrapper
gazebo_initialized = self._gazebo_wrapper.initialize()
assert gazebo_initialized, "Failed to initialize ignition gazebo"
return self._gazebo_wrapper
def main(args=None):
# Set verbosity
logger.set_level(gym.logger.ERROR)
# Create a partial function passing the environment id
make_env = functools.partial(make_env_from_id, env_id=env_id)
# Wrap environment with randomizer
env = ManipulationGazeboEnvRandomizer(env=make_env,
object_random_pose=True,
object_models_rollouts_num=1,
object_random_use_mesh_models=True,
object_random_model_count=3,
ground_model_rollouts_num=1)
# Initialize random seed
env.seed(42)
# Enable rendering
env.render('human')
# Check it
check_env(env, warn=True, skip_render_check=True)
# Step environment for bunch of episodes
for episode in range(100000):
# Initialize returned values
done = False
total_reward = 0
# Reset the environment
observation = env.reset()
# Step through the current episode until it is done
while not done:
# Sample random action
action = env.action_space.sample()
# Step the environment with the random action
observation, reward, done, info = env.step(action)
# Accumulate the reward
total_reward += reward
print(f"Episode #{episode} reward: {total_reward}")
# Cleanup once done
env.close()
def gympp_env(self) -> bindings.IgnitionEnvironment:
if self._env:
return self._env
# Get the metadata
md = self._plugin_metadata
# Register the environment
factory = bindings.GymFactory.Instance()
factory.registerPlugin(md)
# Load the environment from gympp
self._env = factory.make(md.getEnvironmentName())
assert self._env, "Failed to create environment " + md.getEnvironmentName()
# Set the verbosity
logger.set_level(gym.logger.MIN_LEVEL)
# Return the gympp environment
return self._env
def test_joint_controller():
# ==========
# PARAMETERS
# ==========
agent_rate = 100.0
physics_rate = 1000.0
controller_rate = 500.0
plugin_data = bindings.PluginData()
plugin_data.libName = "RobotController"
plugin_data.className = "gympp::plugins::RobotController"
# Find and load the model SDF file
model_sdf_file = resource_finder.find_resource("CartPole/CartPole.urdf")
with open(model_sdf_file, "r") as stream:
model_sdf_string = stream.read()
# Initialize the model data
model_data = bindings.ModelInitData()
model_data.sdfString = model_sdf_string
# Get the model name
model_name = bindings.GazeboWrapper.getModelNameFromSDF(model_sdf_string)
robot_name = model_name
# =============
# CONFIGURATION
# =============
# Create the gazebo wrapper
num_of_iterations = int(physics_rate / agent_rate)
desired_rtf = float(np.finfo(np.float32).max)
gazebo = bindings.GazeboWrapper(num_of_iterations, desired_rtf, physics_rate)
assert gazebo, "Failed to get the gazebo wrapper"
# Set the verbosity
logger.set_level(gym.logger.DEBUG)
# Initialize the world
world_ok = gazebo.setupGazeboWorld("DefaultEmptyWorld.world")
assert world_ok, "Failed to initialize the gazebo world"
# Initialize the ignition gazebo wrapper (creates a paused simulation)
gazebo_initialized = gazebo.initialize()
assert gazebo_initialized, "Failed to initialize ignition gazebo"
# Insert the model
model_ok = gazebo.insertModel(model_data, plugin_data)
assert model_ok, "Failed to insert the model in the simulation"
assert bindings.RobotSingleton_get().exists(robot_name), \
"The robot interface was not registered in the singleton"
# Extract the robot interface from the singleton
robot_weak_ptr = bindings.RobotSingleton_get().getRobot(robot_name)
assert not robot_weak_ptr.expired(), "The Robot object has expired"
assert robot_weak_ptr.lock(), \
"The returned Robot object does not contain a valid interface"
assert robot_weak_ptr.lock().valid(), "The Robot object is not valid"
# Get the pointer to the robot interface
robot = robot_weak_ptr.lock()
# Set the default update rate
robot.setdt(1 / controller_rate)
# Control the cart joint in position
ok_cm = robot.setJointControlMode("linear", bindings.JointControlMode_Position)
assert ok_cm, "Failed to control the cart joint in position"
# Set the PID of the cart joint
pid = bindings.PID(10000, 1000, 1000)
pid_ok = robot.setJointPID("linear", pid)
assert pid_ok, "Failed to set the PID of the cart joint"
# Reset the robot state
robot.resetJoint("pivot", 0, 0)
robot.resetJoint("linear", 0, 0)
# Generate the cart trajectory
cart_ref = np.fromiter(
(0.2 * np.sin(2 * np.pi * 0.5 * t) for t in np.arange(0, 5, 1 / agent_rate)),
dtype=np.float)
# Initialize the cart position buffer
pos_cart_buffer = np.zeros(np.shape(cart_ref))
for (i, ref) in enumerate(cart_ref):
# Set the references
ok1 = robot.setJointPositionTarget("linear", ref)
assert ok1, "Failed to set joint references"
# Step the simulator
gazebo.run()
# Read the position
pos_cart_buffer[i] = robot.jointPosition("linear")
# Check that the trajectory was followed correctly
assert np.abs(pos_cart_buffer - cart_ref).sum() / cart_ref.size < 5E-3, \
"The reference trajectory was not tracked correctly"
def get_gazebo_and_robot(rtf: float,
physics_rate: float,
agent_rate: float,
# controller_rate: float = None,
) \
-> Tuple[bindings.GazeboWrapper, bindings.Robot]:
# ==========
# PARAMETERS
# ==========
model_sdf = "CartPole/CartPole.sdf"
world_sdf = "DefaultEmptyWorld.world"
plugin_data = bindings.PluginData()
plugin_data.setLibName("RobotController")
plugin_data.setClassName("gympp::plugins::RobotController")
# Find and load the model SDF file
model_sdf_file = resource_finder.find_resource(model_sdf)
with open(model_sdf_file, "r") as stream:
model_sdf_string = stream.read()
# Initialize the model data
model_data = bindings.ModelInitData()
model_data.setSdfString(model_sdf_string)
# Create a unique model name
letters_and_digits = string.ascii_letters + string.digits
prefix = ''.join(random.choice(letters_and_digits) for _ in range(6))
# Get the model name
model_name = bindings.GazeboWrapper.getModelNameFromSDF(model_sdf_string)
model_data.modelName = prefix + "::" + model_name
robot_name = model_data.modelName
num_of_iterations_per_gazebo_step = physics_rate / agent_rate
assert num_of_iterations_per_gazebo_step == int(
num_of_iterations_per_gazebo_step)
# =============
# CONFIGURATION
# =============
# Create the gazebo wrapper
gazebo = bindings.GazeboWrapper(int(num_of_iterations_per_gazebo_step),
rtf, physics_rate)
assert gazebo, "Failed to get the gazebo wrapper"
# Set verbosity
logger.set_level(gym.logger.DEBUG)
# Initialize the world
world_ok = gazebo.setupGazeboWorld(world_sdf)
assert world_ok, "Failed to initialize the gazebo world"
# Initialize the ignition gazebo wrapper (creates a paused simulation)
gazebo_initialized = gazebo.initialize()
assert gazebo_initialized, "Failed to initialize ignition gazebo"
# Insert the model
model_ok = gazebo.insertModel(model_data, plugin_data)
assert model_ok, "Failed to insert the model in the simulation"
assert bindings.RobotSingleton_get().exists(robot_name), \
"The robot interface was not registered in the singleton"
# Extract the robot interface from the singleton
robot_weak_ptr = bindings.RobotSingleton_get().getRobot(robot_name)
assert not robot_weak_ptr.expired(), "The Robot object has expired"
assert robot_weak_ptr.lock(), \
"The returned Robot object does not contain a valid interface"
assert robot_weak_ptr.lock().valid(), "The Robot object is not valid"
# Get the pointer to the robot interface
robot = robot_weak_ptr.lock()
# Set the joint controller period
robot.setdt(0.001 if physics_rate < 1000 else physics_rate)
# Return the simulator and the robot object
return gazebo, robot
# GNU Lesser General Public License v2.1 or any later version.
import gym
import time
import pytest
import string
import random
import itertools
import numpy as np
import gym_ignition
from typing import List, Tuple
from gym_ignition import gympp_bindings as bindings
from gym_ignition.utils import logger, resource_finder
# Set verbosity
logger.set_level(gym.logger.ERROR)
def get_gazebo_and_robot(rtf: float,
physics_rate: float,
agent_rate: float,
# controller_rate: float = None,
) \
-> Tuple[bindings.GazeboWrapper, bindings.Robot]:
# ==========
# PARAMETERS
# ==========
model_sdf = "CartPole/CartPole.sdf"
world_sdf = "DefaultEmptyWorld.world"
# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
# This software may be modified and distributed under the terms of the
# GNU Lesser General Public License v2.1 or any later version.
import gym
import pytest
from gym_ignition.utils import logger
# Set verbosity
logger.set_level(gym.logger.DEBUG)
def template_run_environment(env_name):
logger.info(f"Testing environment '{env_name}'")
env = gym.make(env_name)
assert env, f"Failed to create '{env_name}' environment"
observation = env.observation_space.sample()
assert observation.size > 0, "The sampled observation is empty"
observation = env.reset()
assert observation.size > 0, "The observation is empty"
for _ in range(10):
action = env.action_space.sample()
state, reward, done, _ = env.step(action)
assert state.size > 0, "The environment didn't return a valid state"
env.close()
