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