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()