def reset(self) -> Observation:
# Initialize gazebo if not yet done
gazebo = self.gazebo
assert gazebo, "Gazebo object not valid"
# Remove the robot and insert a new one
if not self._first_run:
logger.debug("Hard reset: deleting the robot")
self.task.robot.delete_simulated_robot()
# Execute a dummy step to process model removal
self.gazebo.run()
logger.debug("Hard reset: creating new robot")
self.task.robot = self._get_robot()
else:
self._first_run = False
# Reset the environment
ok_reset = self.task.reset_task()
assert ok_reset, "Failed to reset the task"
# Get the observation
observation = self.task.get_observation()
if not self.observation_space.contains(observation):
logger.warn(
"The observation does not belong to the observation space")
return Observation(observation)
def reset(self) -> Observation:
# Initialize gazebo if not yet done
gazebo = self.gazebo
assert gazebo, "Gazebo object not valid"
# Remove the model and insert it again. This is the reset strategy for
# floating-base robots. Resetting the joint state, instead, is sufficient to
# reset fixed-based robots. Though, while avoiding the model deletion might
# provide better performance, we should be sure that all the internal buffers
# (e.g. those related to the low-level PIDs) are correctly re-initialized.
if self._hard_reset and self.task.has_robot():
if not self._first_run:
logger.debug("Hard reset: deleting the robot")
self.task.robot.delete_simulated_robot()
logger.debug("Hard reset: creating new robot")
self.task.robot = self._get_robot()
else:
self._first_run = False
# Reset the environment
ok_reset = self.task.reset_task()
assert ok_reset, "Failed to reset the task"
# Get the observation
observation = self.task.get_observation()
if not self.observation_space.contains(observation):
logger.warn(
"The observation does not belong to the observation space")
return Observation(observation)
def reset(self) -> Observation:
# Initialize pybullet if not yet done
p = self.pybullet
assert p, "PyBullet object not valid"
if self._hard_reset and self.task.has_robot():
if not self._first_run:
logger.debug("Hard reset: deleting the robot")
self.task.robot.delete_simulated_robot()
logger.debug("Hard reset: creating new robot")
self.task.robot = self._get_robot()
else:
self._first_run = False
# Reset the environment
ok_reset = self.task.reset_task()
assert ok_reset, "Failed to reset the task"
# Get the observation
observation = self.task.get_observation()
if not self.observation_space.contains(observation):
logger.warn(
"The observation does not belong to the observation space")
return Observation(observation)
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 gympp_robot(self):
if self._gympp_robot:
assert not self._gympp_robot.expired(), "The Robot object has expired"
assert self._gympp_robot.lock(), "The Robot object is empty"
assert self._gympp_robot.lock().valid(), "The Robot object is not valid"
return self._gympp_robot.lock()
# Find and load the model SDF file
sdf_file = resource_finder.find_resource(self.model_file)
with open(sdf_file, "r") as stream:
sdf_string = stream.read()
# Get the model name
original_name = bindings.GazeboWrapper.getModelNameFromSDF(sdf_string)
assert original_name, f"Failed to get model name from file {self.model_file}"
# Create a unique robot name
self._robot_name = self._prefix + "::" + original_name
# Initialize the model data
model_data = bindings.ModelInitData()
model_data.setModelName(self._robot_name)
model_data.setSdfString(sdf_string)
model_data.setPosition([0., 0, 0]) # TODO: default initial position
model_data.setOrientation([1., 0, 0, 0]) # TODO: default initial orientation
# Initialize robot controller plugin
plugin_data = bindings.PluginData()
plugin_data.setLibName("RobotController")
plugin_data.setClassName("gympp::plugins::RobotController")
# Insert the model
ok_model = self._gazebo.insertModel(model_data, plugin_data)
assert ok_model, "Failed to insert the model"
# Extract the robot from the singleton
self._gympp_robot = bindings.RobotSingleton_get().getRobot(self._robot_name)
# The robot is a weak pointer. Check that it is valid.
assert not self._gympp_robot.expired(), "The Robot object has expired"
assert self._gympp_robot.lock(), \
"The returned Robot object does not contain a valid interface"
assert self._gympp_robot.lock().valid(), "The Robot object is not valid"
if self._controller_rate:
logger.debug("Robot controller rate: {} Hz".format(self._controller_rate))
ok_dt = self._gympp_robot.lock().setdt(1 / self._controller_rate)
assert ok_dt, "Failed to set the robot controller period"
logger.debug(f"IgnitionRobot '{self._gympp_robot.lock().name()}' added to the "
"simulation")
return self._gympp_robot.lock()
def _load_model(self, filename: str, **kwargs) -> int:
logger.debug(f"Loading model {filename}")
# Get the file extension
extension = os.path.splitext(filename)[1][1:]
if extension == "sdf":
model_id = self._pybullet.loadSDF(filename, **kwargs)[0]
else:
model_id = self._pybullet.loadURDF(
filename, flags=pybullet.URDF_USE_INERTIA_FROM_FILE, **kwargs)
return model_id
def find_resource(file_name: str) -> str:
file_abs_path = ""
global GYM_IGNITION_DATA_PATH
logger.debug(f"Looking for file '{file_name}'")
# Handle if the path is absolute
if os.path.isabs(file_name):
if isfile(file_name):
logger.debug(f" Found resource: '{file_name}'")
return file_name
else:
raise Exception(f"Failed to find resource '{file_name}'")
# Handle if the path is relative
for path in GYM_IGNITION_DATA_PATH:
logger.debug(f" Exploring folder '{path}'")
path_with_slash = path if path[-1] == '/' else path + "/"
candidate_abs_path = path_with_slash + file_name
if isfile(candidate_abs_path):
logger.debug(f" Found resource: '{candidate_abs_path}'")
file_abs_path = candidate_abs_path
break
if not file_abs_path:
raise Exception(f"Failed to find resource '{file_name}'")
return file_abs_path
def add_path(data_path: str) -> None:
if not exists(data_path):
logger.warn(f"The path '{data_path}' does not exist. Not added to the data path.")
return
global GYM_IGNITION_DATA_PATH
for path in GYM_IGNITION_DATA_PATH:
if path == data_path:
logger.debug(f"The path '{data_path}' is already present in the data path")
return
logger.debug(f"Adding new search path: '{data_path}'")
GYM_IGNITION_DATA_PATH.append(data_path)
def pybullet(self) -> bullet_client.BulletClient:
if self._pybullet is not None:
return self._pybullet
logger.debug("Creating PyBullet simulator")
if self._render_enabled:
self._pybullet = bullet_client.BulletClient(pybullet.GUI)
else:
# Connects to an existing instance or, if it fails, creates an headless
# simulation (DIRECT)
self._pybullet = bullet_client.BulletClient()
assert self._pybullet, "Failed to create the bullet client"
# Find the ground plane
resource_finder.add_path(pybullet_data.getDataPath())
world_abs_path = resource_finder.find_resource(self._world)
# Load the ground plane
self._plane_id = self._load_model(world_abs_path)
# Configure the physics engine
self._pybullet.setGravity(0, 0, -9.81)
self._pybullet.setPhysicsEngineParameter(numSolverIterations=10)
# Configure the physics engine with a single big time step divided in multiple
# substeps. As an alternative, we could use a single substep and step the
# simulation multiple times.
self._pybullet.setTimeStep(1.0 / self._physics_rate /
self._num_of_physics_steps)
self._pybullet.setPhysicsEngineParameter(
numSubSteps=self._num_of_physics_steps)
# Disable real-time update. We step the simulation when needed.
self._pybullet.setRealTimeSimulation(0)
logger.info("PyBullet Physic Engine Parameters:")
logger.info(str(self._pybullet.getPhysicsEngineParameters()))
step_time = 1.0 / self._physics_rate / self._rtf
logger.info(f"Nominal step time: {step_time} seconds")
logger.debug("PyBullet simulator created")
return self._pybullet
def setup_gazebo_env_vars() -> None:
# Configure the environment
ign_gazebo_resource_path = os.environ.get("IGN_GAZEBO_RESOURCE_PATH")
ign_gazebo_system_plugin_path = os.environ.get("IGN_GAZEBO_SYSTEM_PLUGIN_PATH")
if not ign_gazebo_resource_path:
ign_gazebo_resource_path = ""
if not ign_gazebo_system_plugin_path:
ign_gazebo_system_plugin_path = ""
if misc.installed_in_editable_mode():
logger.debug("Developer setup")
# Detect the install prefix
import gympp_bindings
site_packages_path = Path(gympp_bindings.__file__).parent
install_prefix = site_packages_path.parent.parent.parent
logger.debug(f"Detected install prefix: '{install_prefix}'")
# Add the plugins path
ign_gazebo_system_plugin_path += f":{install_prefix}/lib/gympp/plugins"
# Add the worlds and models path
ign_gazebo_resource_path += f":{install_prefix}/share/gympp/gazebo/worlds"
ign_gazebo_resource_path += f":{install_prefix}/share/gympp/gazebo/models"
else:
logger.debug("User setup")
# Add the plugins path
import gym_ignition
ign_gazebo_system_plugin_path += f":{gym_ignition.__path__[0]}/plugins"
# Add the worlds and models path
import gym_ignition_data
data_path = gym_ignition_data.__path__[0]
ign_gazebo_resource_path += f":{data_path}:/{data_path}/worlds"
os.environ["IGN_GAZEBO_RESOURCE_PATH"] = ign_gazebo_resource_path
os.environ["IGN_GAZEBO_SYSTEM_PLUGIN_PATH"] = ign_gazebo_system_plugin_path
logger.debug(f"IGN_GAZEBO_RESOURCE_PATH={ign_gazebo_resource_path}")
logger.debug(f"IGN_GAZEBO_SYSTEM_PLUGIN_PATH={ign_gazebo_system_plugin_path}")
def _get_robot(self):
if not self.gazebo:
raise Exception("Failed to instantiate the gazebo simulator")
# Build the robot object
logger.debug("Creating the robot object")
robot = self._robot_cls(model_file=self._model,
gazebo=self.gazebo,
**self._kwargs)
assert isinstance(robot, robot_abc.RobotABC), \
"'robot' object must inherit from RobotABC"
# Check the requested robot features
if self.task.robot_features:
self.task.robot_features.has_all_features(robot)
if not robot.valid():
raise Exception("The robot is not valid")
return robot
def _get_robot(self) -> robot_abc.RobotABC:
if not self.pybullet:
raise Exception("Failed to instantiate the pybullet simulator")
if not issubclass(self._robot_cls,
robots.pybullet_robot.PyBulletRobot):
raise Exception("The 'robot_cls' must inherit from PyBulletRobot")
# Build the robot object
logger.debug("Creating the robot object")
robot = self._robot_cls(plane_id=self._plane_id,
model_file=self._model,
p=self._pybullet,
**self._kwargs)
if self.task.robot_features:
self.task.robot_features.has_all_features(robot)
if not robot.valid():
raise Exception("The robot is not valid")
logger.debug("Robot object created")
return robot
def template_test(
rtf: float,
physics_rate: float,
agent_rate: float,
# controller_rate: float = None,
) -> None:
# Get the simulator and the robot objects
gazebo, robot = get_gazebo_and_robot(
rtf=rtf,
physics_rate=physics_rate,
agent_rate=agent_rate,
# controller_rate=None, # Does not matter
)
# Trajectory specifications
trajectory_dt = 1 / agent_rate
tot_simulated_seconds = 2
# Generate the cart trajectory. This is analogous of setting an action containing
# the cart references, hence it is related to the agent rate.
cart_ref = np.fromiter(
(0.2 * np.sin(2 * np.pi * 0.5 * t)
for t in np.arange(0, tot_simulated_seconds, trajectory_dt)),
dtype=np.float)
avg_time_per_step = 0.0
start = time.time()
logger.debug(f"Simulating {cart_ref.size} steps")
for i, ref in enumerate(cart_ref):
# Set the references
ok_ref = robot.setJointPositionTarget("linear", ref)
assert ok_ref, "Failed to set joint references"
# Step the simulator
now = time.time()
gazebo.run()
this_step = time.time() - now
avg_time_per_step = avg_time_per_step + 0.1 * (this_step -
avg_time_per_step)
stop = time.time()
elapsed_time = stop - start
# Compute useful quantities
# simulation_dt = 1 / simulation_rate
number_of_actions = tot_simulated_seconds / trajectory_dt
physics_iterations_per_run = physics_rate / agent_rate
simulation_dt = 1 / (physics_rate * rtf)
assert number_of_actions == cart_ref.size
print()
print("Elapsed time = {}".format(elapsed_time))
print("Average step time = {}".format(avg_time_per_step))
print("Number of actions: = {}".format(tot_simulated_seconds /
trajectory_dt))
print("Physics iteration per simulator step = {}".format(
physics_iterations_per_run))
print()
# Check if the average time per step matches what expected
assert almost_equal(first=avg_time_per_step,
second=simulation_dt * physics_iterations_per_run,
epsilon=avg_time_per_step * 0.5)
# Check if the total time of the simulation matched what expected
assert almost_equal(first=elapsed_time,
second=number_of_actions * simulation_dt *
physics_iterations_per_run,
epsilon=elapsed_time * 0.5)
# Terminate simulation
gazebo.close()
def template_test(
rtf: float,
physics_rate: float,
agent_rate: float,
) -> None:
# Get the simulator
iterations = int(physics_rate / agent_rate)
gazebo = utils.Gazebo(physics_rate=physics_rate,
iterations=iterations,
rtf=rtf)
# Get the cartpole robot
robot = utils.get_cartpole(gazebo)
assert robot.valid(), "The robot object is not valid"
# Configure the cartpole
ok_cm = robot.set_joint_control_mode("linear", JointControlMode.POSITION)
assert ok_cm, "Failed to set the control mode"
# Set the joint controller period
robot.set_dt(0.001 if physics_rate < 1000 else physics_rate)
# Trajectory specifications
trajectory_dt = 1 / agent_rate
tot_simulated_seconds = 2
# Generate the cart trajectory. This is analogous of setting an action containing
# the cart references, hence it is related to the agent rate.
cart_ref = np.fromiter(
(0.2 * np.sin(2 * np.pi * 0.5 * t)
for t in np.arange(0, tot_simulated_seconds, trajectory_dt)),
dtype=np.float)
avg_time_per_step = 0.0
start = time.time()
logger.debug(f"Simulating {cart_ref.size} steps")
for i, ref in enumerate(cart_ref):
# Set the references
ok_ref = robot.set_joint_position("linear", ref)
assert ok_ref, "Failed to set joint references"
# Step the simulator
now = time.time()
gazebo.step()
this_step = time.time() - now
avg_time_per_step = avg_time_per_step + 0.1 * (this_step -
avg_time_per_step)
stop = time.time()
elapsed_time = stop - start
# Compute useful quantities
number_of_actions = tot_simulated_seconds / trajectory_dt
physics_iterations_per_run = physics_rate / agent_rate
simulation_dt = 1 / (physics_rate * rtf)
assert number_of_actions == cart_ref.size
print()
print(f"Elapsed time = {elapsed_time}")
print(f"Average step time = {avg_time_per_step}")
print("Number of actions: = {}".format(tot_simulated_seconds /
trajectory_dt))
print(
f"Physics iteration per simulator step = {physics_iterations_per_run}")
print()
# Check if the average time per step matches what expected
assert almost_equal(first=avg_time_per_step,
second=simulation_dt * physics_iterations_per_run,
epsilon=avg_time_per_step * 0.5)
# Check if the total time of the simulation matched what expected
assert almost_equal(first=elapsed_time,
second=number_of_actions * simulation_dt *
physics_iterations_per_run,
epsilon=elapsed_time * 0.5)
# Terminate simulation
gazebo.close()
def __init__(self,
task_cls: type,
robot_cls: type,
model: str,
rtf: float,
agent_rate: float,
physics_rate: float,
world: str = "plane_implicit.urdf",
hard_reset: bool = True,
**kwargs):
# Save the keyworded arguments.
# We use them to build the task and the robot objects, and allow custom class
# to accept user-defined parameters.
self._kwargs = kwargs
# Store the type of the class that provides Robot interface
self._robot_cls = robot_cls
# Delete and create a new robot every environment reset
self._first_run = True
self._hard_reset = hard_reset
# URDF or SDF model files
self._world = world
self._model = model
# Simulation attributes
self._rtf = rtf
self._now = None
self._bias = 0.0
self._physics_rate = physics_rate
self._plane_id = None
self._pybullet = None
self._render_enabled = False
self._render_called = False
# Calculate the rate between agent and physics rate
physics_steps_per_agent_update = physics_rate / agent_rate
self._num_of_physics_steps = int(physics_steps_per_agent_update)
assert physics_rate >= agent_rate, "Agent cannot run faster than physics"
# If there is an incompatibility between the agent and physics rates, round the
# iterations and notify the user
if physics_steps_per_agent_update != round(
physics_steps_per_agent_update):
self._num_of_physics_steps = round(physics_steps_per_agent_update)
logger.warn("The real physics rate will be {} Hz".format(
agent_rate * self._num_of_physics_steps))
logger.debug(f"RTF = {rtf}")
logger.debug(f"Agent rate = {agent_rate} Hz")
logger.debug(
f"Physics rate = {agent_rate * self._num_of_physics_steps} Hz")
logger.debug("Initializing the Task")
task = task_cls(**kwargs)
assert isinstance(task, base.task.Task), \
"'task_cls' object must inherit from Task"
# Wrap the task with this runtime
super().__init__(task=task, agent_rate=agent_rate)
# Initialize the simulator and the robot
self.task.robot = self._get_robot()
# Initialize the spaces
self.task.action_space, self.task.observation_space = self.task.create_spaces(
)
# Seed the environment
self.seed()
for epoch in range(10):
# Reset the environment
observation = env.reset()
# Initialize returned values
done = False
totalReward = 0
while not done:
# Execute a random action
action = env.action_space.sample()
observation, reward, done, _ = env.step(action)
# Render the environment
# It is not required to call this in the loop
# env.render('human')
# Accumulate the reward
totalReward += reward
# Print the observation
msg = ""
for value in observation:
msg += "\t%.6f" % value
logger.debug(msg)
logger.info("Total reward for episode #{}: {}".format(epoch, totalReward))
env.close()
def gympp_robot(self) -> bindings.Robot:
if self._gympp_robot:
return self._gympp_robot
# Find and load the model SDF file
sdf_file = resource_finder.find_resource(self.model_file)
with open(sdf_file, "r") as stream:
sdf_string = stream.read()
# Get the model name
original_name = bindings.GazeboWrapper.getModelNameFromSDF(sdf_string)
assert original_name, f"Failed to get model name from file {self.model_file}"
# Create a unique robot name
self._robot_name = self._prefix + "::" + original_name
initial_base_pose, initial_base_orientation = self.initial_base_pose()
# Initialize the model data
model_data = bindings.ModelInitData()
model_data.modelName = self._robot_name
model_data.sdfString = sdf_string
model_data.fixedPose = not self.is_floating_base()
model_data.position = initial_base_pose.tolist()
model_data.orientation = initial_base_orientation.tolist()
if self._base_frame is not None:
model_data.baseLink = self._base_frame
# Initialize robot controller plugin
plugin_data = bindings.PluginData()
plugin_data.libName = "RobotController"
plugin_data.className = "gympp::plugins::RobotController"
# Insert the model
ok_model = self._gazebo.insertModel(model_data, plugin_data)
assert ok_model, "Failed to insert the model"
# Extract the robot from the singleton
gympp_robot_weak = bindings.RobotSingleton_get().getRobot(
self._robot_name)
# The robot is a weak pointer. Check that it is valid.
assert not gympp_robot_weak.expired(), "The Robot object has expired"
assert gympp_robot_weak.lock(), \
"The returned Robot object does not contain a valid interface"
self._gympp_robot = gympp_robot_weak.lock()
assert self._gympp_robot.valid(), "The Robot object is not valid"
if self._controller_rate is not None:
logger.debug(f"Robot controller rate: {self._controller_rate} Hz")
ok_dt = self._gympp_robot.setdt(1 / self._controller_rate)
assert ok_dt, "Failed to set the robot controller period"
s0 = self.initial_joint_positions()
sdot0 = self.initial_joint_velocities()
joint_names = list(self._gympp_robot.jointNames())
assert s0.size == len(joint_names)
assert sdot0.size == len(joint_names)
for idx, name in enumerate(joint_names):
ok_reset = self._gympp_robot.resetJoint(name, s0[idx], sdot0[idx])
assert ok_reset, f"Failed to initialize the state of joint '{name}'"
logger.debug(
f"GazeboRobot '{self._gympp_robot.name()}' added to the simulation"
)
return self._gympp_robot
