diff --git a/sofagym/AbstractEnv.py b/sofagym/AbstractEnv.py index 20b42c8..64f7e06 100644 --- a/sofagym/AbstractEnv.py +++ b/sofagym/AbstractEnv.py @@ -19,8 +19,13 @@ import splib3 -from sofagym.viewer import Viewer from sofagym.rpc_server import start_server, add_new_step, get_result, clean_registry, close_scene +from sofagym.viewer import Viewer + +import importlib + +import Sofa +import SofaRuntime class AbstractEnv(gym.Env): @@ -39,7 +44,6 @@ class AbstractEnv(gym.Env): while an action is still ongoing. close: Terminate the simulation. configure: Add element in the configuration. - clean: clean the registery. _formataction.. : transforme the type of action to use server. Arguments: @@ -112,7 +116,7 @@ class AbstractEnv(gym.Env): """ - def __init__(self, config=None, render_mode: Optional[str] = None): + def __init__(self, default_config, config=None, render_mode: Optional[str]=None, root=None): """ Classic initialization of a class in python. @@ -127,48 +131,63 @@ def __init__(self, config=None, render_mode: Optional[str] = None): """ # Define a DEFAULT_CONFIG in sub-class. - self.config = copy.deepcopy(self.DEFAULT_CONFIG) + self.config = copy.deepcopy(default_config) self.config["dt"] = self.config.get('dt', 0.01) if config is not None: self.config.update(config) - self.initialization() + self.scene = self.config['scene'] - self.render_mode = render_mode - - def initialization(self): - """Initialization of all parameters. - - Parameters: - ---------- - None. + self._getState = importlib.import_module("sofagym.envs."+self.scene+"."+self.scene+"Toolbox").getState + self._getReward = importlib.import_module("sofagym.envs."+self.scene+"."+self.scene+"Toolbox").getReward + self._startCmd = importlib.import_module("sofagym.envs."+self.scene+"."+self.scene+"Toolbox").startCmd + self._getPos = importlib.import_module("sofagym.envs."+self.scene+"."+self.scene+"Toolbox").getPos + + try: + self.create_scene = importlib.import_module("sofagym.envs."+self.scene+"." + self.scene + "Scene").createScene + except Exception as exc: + print("sofagym.envs."+self.scene+"." + self.scene + "Scene") + raise NotImplementedError("Importing your SOFA Scene Failed") from exc - Returns: - ------- - None. - """ + self.viewer = None + self.render_mode = render_mode - self.goalList = None - self.goal = None self.past_actions = [] - + self.pos = [] + self.past_pos = [] + self.num_envs = 40 self.np_random = None self.seed(self.config['seed']) - self.viewer = None - self.automatic_rendering_callback = None - - self.timer = 0 self.timeout = self.config["timeout"] - # Start the server which distributes the calculations to its clients - start_server(self.config) + self.init_save_paths() + + self.root = root + + self.init_states = None + + self.goal = None + + self.nb_actions = self.config["nb_actions"] + self.dim_state = self.config["dim_state"] + + def init_save_paths(self): + """Create directories to save results and images. + + Parameters: + ---------- + None. + Returns: + ------- + None. + """ if 'save_data' in self.config and self.config['save_data']: save_path_results = self.config['save_path']+"/data" os.makedirs(save_path_results, exist_ok=True) @@ -183,6 +202,38 @@ def initialization(self): self.configure({"save_path_image": save_path_image, "save_path_results": save_path_results}) + def init_root(self): + self.init_simulation() + + def initialize_states(self): + if self.config["randomize_states"]: + self.init_states = self.randomize_init_states() + self.config.update({'init_states': list(self.init_states)}) + else: + self.init_states = self.config["init_states"] + + def randomize_init_states(self): + """Randomize initial states. + + Returns: + ------- + init_states: list + List of random initial states for the environment. + + Note: + ---- + This method should be implemented according to needed random initialization. + """ + return self.config["init_states"] + + def init_goal(self): + # Set a new random goal from the list + goalList = self.config["goalList"] + id_goal = self.np_random.choice(range(len(goalList))) + self.config.update({'goal_node': id_goal}) + self.goal = goalList[id_goal] + self.config.update({'goalPos': self.goal}) + def seed(self, seed=None): """ Computes the random generators of the environment. @@ -200,6 +251,19 @@ def seed(self, seed=None): self.np_random, seed = seeding.np_random(seed) return [seed] + def get_available_actions(self): + """Gives the actions available in the environment. + + Parameters: + ---------- + None. + + Returns: + ------- + list of the action available in the environment. + """ + return self.action_space + def _formataction(self, action): """Change the type of action to be in [list, float, int]. @@ -258,23 +322,6 @@ def _formatactionDict(self, action): return action - def clean(self): - """Function to clean the registery . - - Close clients who are processing unused sequences of actions (for - planning) - - Parameters: - ---------- - None. - - Returns: - ------- - None. - """ - - clean_registry(self.past_actions) - def step(self, action): """Executes one action in the environment. @@ -298,32 +345,26 @@ def step(self, action): """ - # assert self.action_space.contains(action), "%r (%s) invalid" % (action, type(action)) action = self._formataction(action) - # Pass the actions to the server to launch the simulation. - result_id = add_new_step(self.past_actions, action) - self.past_actions.append(action) + self.pos = self.step_simulation(action) - # Request results from the server. - # print("[INFO] >>> Result id:", result_id) - results = get_result(result_id, timeout=self.timeout) + self.past_actions.append(action) + self.past_pos.append(self.pos) - obs = np.array(results["observation"]) # to work with baseline - reward = results["reward"] - done = results["done"] + obs = np.array(self._getState(self.root), dtype=np.float32) + done, reward = self._getReward(self.root) # Avoid long explorations by using a timer. self.timer += 1 if self.timer >= self.config["timer_limit"]: # reward = -150 truncated = True - info={}#(not use here) + + info = {} #(not use here) - if self.config["planning"]: - self.clean() return obs, reward, done, info def async_step(self, action): @@ -376,22 +417,22 @@ def reset(self): obs, info """ - self.clean() self.viewer = None splib3.animation.animate.manager = None - if not self.goalList: - self.goalList = self.config["goalList"] - - # Set a new random goal from the list - id_goal = self.np_random.choice(range(len(self.goalList))) - self.config.update({'goal_node': id_goal}) - self.goal = self.goalList[id_goal] self.timer = 0 self.past_actions = [] + self.pos = [] + self.past_pos = [] + + Sofa.Simulation.reset(self.root) + self.root = None + self.init_simulation() - return + obs = np.array(self._getState(self.root), dtype=np.float32) + + return obs def render(self, mode): """See the current state of the environment. @@ -411,40 +452,16 @@ def render(self, mode): self.render_mode = mode # Define the viewer at the first run of render. - if not self.viewer: + if self.viewer is None: display_size = self.config["display_size"] # Sim display if 'zFar' in self.config: zFar = self.config['zFar'] else: zFar = 0 - self.viewer = Viewer(self, display_size, zFar=zFar, save_path=self.config["save_path_image"]) + self.viewer = Viewer(self, self.root, display_size, zFar=zFar, save_path=self.config["save_path_image"]) # Use the viewer to display the environment. return self.viewer.render() - def _automatic_rendering(self): - """Automatically render the intermediate frames while an action is still ongoing. - - This allows to render the whole video and not only single steps - corresponding to agent decision-making. - If a callback has been set, use it to perform the rendering. This is - useful for the environment wrappers such as video-recording monitor that - need to access these intermediate renderings. - - Parameters: - ---------- - None. - - Returns: - ------- - None. - - """ - if self.viewer is not None: - if self.automatic_rendering_callback: - self.automatic_rendering_callback() - else: - self.render() - def close(self): """Terminate simulation. @@ -460,8 +477,7 @@ def close(self): """ if self.viewer is not None: self.viewer.close() - - close_scene() + print("All clients are closed. Bye Bye.") def configure(self, config): @@ -478,3 +494,215 @@ def configure(self, config): """ self.config.update(config) + + def init_simulation(self, mode='simu_and_visu'): + """Function to create scene and initialize all variables. + + Parameters: + ---------- + config: Dictionary. + Configuration of the environment. + _startCmd: function + Initialize the command. + mode: string, default = 'simu_and_visu' + Init a scene with or without visu and computations. + In ['simu', 'visu', 'simu_and_visu'] + + Returns: + ------- + root: + The loaded and initialized scene. + + """ + # Load the scene + self.root = Sofa.Core.Node("root") + + SofaRuntime.importPlugin("Sofa.Component") + self.create_scene(self.root, self.config, mode=mode) + Sofa.Simulation.init(self.root) + + # Realise action from history + if self.config['start_from_history'] is not None and self._startCmd is not None: + print(">> Start from history ...") + render = self.config['render'] + self.config.update({'render': 0}) + + for action in self.config['start_from_history']: + self.step_simulation(action) + + self.config.update({'render': render}) + print(">> ... Done.") + + if self.config["randomize_states"]: + self.root.StateInitializer.init_state(self.config["init_states"]) + + if 'time_before_start' in self.config: + print(">> Time before start:", self.config["time_before_start"], "steps. Initialization ...") + for _ in range(self.config["time_before_start"]): + Sofa.Simulation.animate(self.root, self.config["dt"]) + print(">> ... Done.") + + # Update Reward and GoalSetter + if self.config["goal"]: + self.root.GoalSetter.update(self.goal) + + self.root.Reward.update(self.goal) + + def step_simulation(self, action): + """Realise one step in the simulation. + + Apply action and execute 5 times scale_factor simulation steps of dt s. + + Parameters: + ---------- + root: + The scene. + config: Dictionary + The configuration of the environment. + action: int + The action to apply in the environment. + _startCmd: function + Initialize the command. + _getPos: function + Get the position of the object in the scene. + + Returns: + -------- + position(s): list + The positions of object(s) in the scene. + + """ + if self.config["goal"]: + goal = self.config['goalPos'] + self.root.GoalSetter.set_mo_pos(goal) + + render = self.config['render'] + surface_size = self.config['display_size'] + + # Create the command from action + self._startCmd(self.root, action, self.config["dt"]*(self.config["scale_factor"]-1)) + pos = [] + # Realise scale_factor simulation steps of 0.01 s + for _ in range(self.config["scale_factor"]): + Sofa.Simulation.animate(self.root, self.config["dt"]) + + #if render == 2: + # pos.append(self._getPos(self.root)) + # if self.viewer is not None: + # self.viewer.render_simulation(self.root) + + if render == 1: + pos.append(self._getPos(self.root)) + + return pos + + + + +class ServerEnv(AbstractEnv): + def __init__(self, default_config, config=None, render_mode: Optional[str]=None, root=None): + super().__init__(default_config, config, root=root) + + # Start the server which distributes the calculations to its clients + start_server(self.config) + + def step(self, action): + """Executes one action in the environment. + + Apply action and execute scale_factor simulation steps of 0.01 s. + + Parameters: + ---------- + action: int + Action applied in the environment. + + Returns: + ------- + obs(ObsType): + The new state of the agent. + reward(float): + The reward obtain after applying the action in the current state. + done(bool): + Whether the agent reaches the terminal state + info(dict): + additional information (not used here) + """ + # assert self.action_space.contains(action), "%r (%s) invalid" % (action, type(action)) + + action = self._formataction(action) + + # Pass the actions to the server to launch the simulation. + result_id = add_new_step(self.past_actions, action) + self.past_actions.append(action) + + # Request results from the server. + # print("[INFO] >>> Result id:", result_id) + results = get_result(result_id, timeout=self.timeout) + + obs = np.array(results["observation"]) # to work with baseline + reward = results["reward"] + done = results["done"] + + # Avoid long explorations by using a timer. + self.timer += 1 + if self.timer >= self.config["timer_limit"]: + # reward = -150 + truncated = True + info={}#(not use here) + + if self.config["planning"]: + self.clean() + return obs, reward, done, info + + def reset(self): + """Reset simulation. + + Parameters: + ---------- + None. + + Returns: + ------- + obs, info + """ + self.clean() + self.viewer = None + + splib3.animation.animate.manager = None + + self.timer = 0 + self.past_actions = [] + + return + + def clean(self): + """Function to clean the registery . + + Close clients who are processing unused sequences of actions (for + planning) + + Parameters: + ---------- + None. + + Returns: + ------- + None. + """ + clean_registry(self.past_actions) + + def close(self): + """Terminate simulation. + + Close the viewer and the scene. + + Parametres: + ---------- + None. + + Returns: + ------- + None. + """ + super().close() + close_scene() diff --git a/sofagym/envs/BubbleMotion/BubbleMotionEnv.py b/sofagym/envs/BubbleMotion/BubbleMotionEnv.py index 5f0d33f..67151c9 100644 --- a/sofagym/envs/BubbleMotion/BubbleMotionEnv.py +++ b/sofagym/envs/BubbleMotion/BubbleMotionEnv.py @@ -8,14 +8,16 @@ __copyright__ = "(c) 2021, Inria" __date__ = "Feb 3 2021" -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces import os, sys import numpy as np -class BubbleMotionEnv(AbstractEnv): +from typing import Optional + +class BubbleMotionEnv: """Sub-class of AbstractEnv, dedicated to the gripper scene. See the class AbstractEnv for arguments and methods. @@ -23,10 +25,12 @@ class BubbleMotionEnv(AbstractEnv): #Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 15 DEFAULT_CONFIG = {"scene": "BubbleMotion", "deterministic": True, "source": [5, -5, 20], "target": [5, 5, 0], + "goal": True, "goalList": [[7, 0, 20]], "start_node": None, "scale_factor": 20, @@ -46,55 +50,69 @@ class BubbleMotionEnv(AbstractEnv): "seed": None, "dt": 0.01, "max_pressure": 40, - "board_dim": 8 + "board_dim": 8, + "nb_actions": -1, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - - def __init__(self, config = None): - super().__init__(config) - nb_actions = -1 + + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) + + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + low = np.array([-1]*9) high = np.array([1]*9) - self.action_space = spaces.Box(low=low, high=high, shape=(9,), dtype='float32') - self.nb_actions = str(nb_actions) - - dim_state = 15 - low_coordinates = np.array([0]*dim_state) - high_coordinates = np.array([80]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') - - - def step(self, action): - return super().step(action) - + self.env.action_space = spaces.Box(low=low, high=high, shape=(9,), dtype=np.float32) + self.nb_actions = str(self.env.nb_actions) + + low_coordinates = np.array([0]*self.env.dim_state) + high_coordinates = np.array([80]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) + + if self.env.root is None and not self.use_server: + self.env.init_root() + + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) + + def initialize_states(self): + self.env.initialize_states() + + bd = self.env.config["board_dim"] + init_pos = [4+(bd-4)*self.env.np_random.random(), 4+(bd-4)*self.env.np_random.random(), 5] + self.env.config.update({'init_pos': init_pos}) + + def init_goal(self): + bd = self.env.config["board_dim"] + pos_goal = [1+bd*self.env.np_random.random(), 1+bd*self.env.np_random.random(), 2] + self.env.goal = pos_goal + self.env.config.update({'goalPos': self.env.goal}) + def reset(self): """Reset simulation. - - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - """ - super().reset() - - self.config.update({'goalPos': self.goal}) - obs = start_scene(self.config, self.nb_actions) + self.initialize_states() - return np.array(obs['observation']) - - def get_available_actions(self): - """Gives the actions available in the environment. - - Parameters: - ---------- - None. - - Returns: - ------- - list of the action available in the environment. - """ - return self.action_space - - + if self.env.config["goal"]: + self.init_goal() + + self.env.reset() + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/BubbleMotion/BubbleMotionScene.py b/sofagym/envs/BubbleMotion/BubbleMotionScene.py index 6d953d2..97dee83 100644 --- a/sofagym/envs/BubbleMotion/BubbleMotionScene.py +++ b/sofagym/envs/BubbleMotion/BubbleMotionScene.py @@ -45,6 +45,7 @@ def createScene(rootNode, config={"source": [5, -5, 20], "zFar":4000, "dt": 0.01, "max_pressure": 40, + "init_pos": [5, 5, 5], "board_dim": 8}, mode='simu_and_visu'): @@ -56,10 +57,10 @@ def createScene(rootNode, config={"source": [5, -5, 20], visu(rootNode, config, position_spot, direction_spot, cutoff=250) bd = config["board_dim"] - pos_goal = [1+bd*np.random.random(), 1+bd*np.random.random(), 2] + pos_goal = config["goalPos"] add_goal_node(rootNode, pos_goal) - init_pos = [4+(bd-4)*np.random.random(), 4+(bd-4)*np.random.random(), 5] + init_pos = config["init_pos"] bubblemotion_config = {'init_pos': init_pos, "dt": config["dt"], "max_pressure": config["max_pressure"]} bubblemotion = BubbleMotion(bubblemotion_config=bubblemotion_config) diff --git a/sofagym/envs/BubbleMotion/BubbleMotionToolbox.py b/sofagym/envs/BubbleMotion/BubbleMotionToolbox.py index 9d4dc71..8f4e5db 100644 --- a/sofagym/envs/BubbleMotion/BubbleMotionToolbox.py +++ b/sofagym/envs/BubbleMotion/BubbleMotionToolbox.py @@ -83,7 +83,7 @@ def getReward(self): return r, dist - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -97,6 +97,7 @@ def update(self): None. """ + self.goal = goal[:2] current_sphere_pos = self._getSpherePos() self.init_goal_dist = float(np.linalg.norm(current_sphere_pos-self.goal)) @@ -158,8 +159,8 @@ def __init__(self, *args, **kwargs): if 'goalPos' in kwargs: self.goalPos = kwargs["goalPos"] - def update(self): - pass + def update(self, goal): + self.goalPos = goal def set_mo_pos(self, goal): pass diff --git a/sofagym/envs/CTR/CTREnv.py b/sofagym/envs/CTR/CTREnv.py index 31153c2..0f175fb 100644 --- a/sofagym/envs/CTR/CTREnv.py +++ b/sofagym/envs/CTR/CTREnv.py @@ -8,17 +8,19 @@ __copyright__ = "(c) 2021, Robocath, CNRS, Inria" __date__ = "Dec 01 2021" -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from sofagym.viewer import LegacyViewer from sofagym.envs.CTR.CTRToolbox import startCmd from gym import spaces -import os +import os, sys import numpy as np +from typing import Optional -class ConcentricTubeRobotEnv(AbstractEnv): + +class ConcentricTubeRobotEnv: """Sub-class of AbstractEnv, dedicated to the trunk scene. See the class AbstractEnv for arguments and methods. @@ -26,6 +28,7 @@ class ConcentricTubeRobotEnv(AbstractEnv): # Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 12 DEFAULT_CONFIG = {"scene": "CTR", "deterministic": True, "source": [-150, 0, 30], @@ -34,6 +37,7 @@ class ConcentricTubeRobotEnv(AbstractEnv): "scale": 30, "rotation": [140.0, 0.0, 0.0], "translation": [0.0, 0.0, 0.0], + "goal": True, "goalList": [[0.0, 0.0, 0.0]], "goalPos": [0.0, 0.0, 0.0], "scale_factor": 10, @@ -47,88 +51,64 @@ class ConcentricTubeRobotEnv(AbstractEnv): "discrete": True, "seed": 0, "start_from_history": None, - "python_version": "python3.8", + "python_version": sys.version, "zFar": 5000, - "dt": 0.01 + "dt": 0.01, + "nb_actions": 12, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config=None): - super().__init__(config) - nb_actions = 12 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) - - dim_state = 12 - low_coordinates = np.array([-1]*dim_state) - high_coordinates = np.array([1]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - self.default_action = 3 + self.initialize_states() - def step(self, action): - if self.viewer: - self.viewer.step(action) + if self.env.config["goal"]: + self.init_goal() - return super().step(action) - - def reset(self): - """Reset simulation. - - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - - """ - super().reset() + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) - y = -20 + 50 * np.random.random() + low_coordinates = np.array([-1]*self.env.dim_state) + high_coordinates = np.array([1]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - self.goal = [0.0, y, abs(y) + 30 * np.random.random()] - - self.config.update({'goalPos': self.goal}) - obs = start_scene(self.config, self.nb_actions) - if self.viewer: - self.viewer.reset() - - self.step(0) - self.step(4) - self.step(8) - - return np.array(obs['observation']) + self.default_action = 3 - def render(self, mode='rgb_array'): - """See the current state of the environment. + if self.env.root is None and not self.use_server: + self.env.init_root() - Get the OpenGL Context to render an image (snapshot) of the simulation - state. + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) - Parameters: - ---------- - mode: string, default = 'rgb_array' - Type of representation. + def init_goal(self): + # Set a new random goal from the list + y = -20 + 50 * self.env.np_random.random() + self.env.goal = [0.0, y, abs(y) + 30 * self.env.np_random.random()] + self.env.config.update({'goalPos': self.env.goal}) - Returns: - ------- - None. + def reset(self): + """Reset simulation. """ - if not self.viewer: - display_size = self.config["display_size"] # Sim display - self.viewer = LegacyViewer(self, display_size, startCmd=startCmd) + self.initialize_states() - # Use the viewer to display the environment. - self.viewer.render() + if self.env.config["goal"]: + self.init_goal() - def get_available_actions(self): - """Gives the actions available in the environment. + self.env.reset() - Parameters: - ---------- - None. - - Returns: - ------- - list of the action available in the environment. - """ - return list(range(int(self.nb_actions))) + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/CTR/CTRScene.py b/sofagym/envs/CTR/CTRScene.py index ac607be..08fd0d7 100644 --- a/sofagym/envs/CTR/CTRScene.py +++ b/sofagym/envs/CTR/CTRScene.py @@ -186,6 +186,7 @@ def add_geometry(root, config, mode_simu=True, mode_visu=True): "rotation": [140.0, 0.0, 0.0], "translation": [0.0, 0.0, 0.0], "goalList": [[0.0, 0.0, 10.0]], + "goalPos": [0.0, 0.0, 0.0], "scale_factor": 10, "timer_limit": 50, "timeout": 30, diff --git a/sofagym/envs/CTR/CTRToolbox.py b/sofagym/envs/CTR/CTRToolbox.py index 1ef487b..5dbfc86 100644 --- a/sofagym/envs/CTR/CTRToolbox.py +++ b/sofagym/envs/CTR/CTRToolbox.py @@ -86,7 +86,7 @@ def getReward(self): return min(3*reward**(1/2), 1.0), current_dist - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -100,7 +100,7 @@ def update(self): None. """ - + self.goal_pos = goal tip = self.root.InstrumentCombined.DOFs.position[-1][:3] self.init_dist = np.linalg.norm(np.array(tip)-np.array(self.goal_pos)) self.prev_dist = self.init_dist @@ -148,7 +148,7 @@ def __init__(self, *args, **kwargs): if kwargs["goalPos"]: self.goalPos = kwargs["goalPos"] - def update(self): + def update(self, goal): """Set the position of the goal. This function is used as an initialization function. @@ -162,6 +162,7 @@ def update(self): None. """ + self.goalPos = goal with self.goalMO.position.writeable() as position: print("update", self.goalPos) position[0] = self.goalPos @@ -377,7 +378,17 @@ def getPos(root): _: list The position(s) of the object(s) of the scene. """ - return + cath_xtip = root.InstrumentCombined.m_ircontroller.xtip.value[0].tolist() + cath_rotation = root.InstrumentCombined.m_ircontroller.rotationInstrument.value[0].tolist() + guide_xtip = root.InstrumentCombined.m_ircontroller.xtip.value[1].tolist() + guide_rotation = root.InstrumentCombined.m_ircontroller.rotationInstrument.value[1].tolist() + coils_xtip = root.InstrumentCombined.m_ircontroller.xtip.value[2].tolist() + coils_rotation = root.InstrumentCombined.m_ircontroller.rotationInstrument.value[2].tolist() + + tip = root.InstrumentCombined.DOFs.position.value.tolist() + collis = root.InstrumentCombined.Collis.CollisionDOFs.position.value.tolist() + + return [cath_xtip, cath_rotation, guide_xtip, guide_rotation, coils_xtip, coils_rotation, tip, collis] def setPos(root, pos): @@ -399,4 +410,17 @@ def setPos(root, pos): Don't forget to init the new value of the position. """ - return + cath_xtip, cath_rotation, guide_xtip, guide_rotation, coils_xtip, coils_rotation, tip, collis = pos + + controller = root.InstrumentCombined.m_ircontroller + with controller.xtip.writeable() as xtip: + xtip[0] = np.array(cath_xtip) + xtip[1] = np.array(guide_xtip) + xtip[2] = np.array(coils_xtip) + + with controller.rotationInstrument.writeable() as rotation: + rotation[0] = np.array(cath_rotation) + rotation[1] = np.array(guide_rotation) + rotation[2] = np.array(coils_rotation) + + root.InstrumentCombined.DOFs.position.value = np.array(tip) diff --git a/sofagym/envs/CartPole/CartPoleEnv.py b/sofagym/envs/CartPole/CartPoleEnv.py index 75280b9..a7683bb 100644 --- a/sofagym/envs/CartPole/CartPoleEnv.py +++ b/sofagym/envs/CartPole/CartPoleEnv.py @@ -1,26 +1,28 @@ import math import os import sys +from typing import Optional import numpy as np from gym import spaces -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene -class CartPoleEnv(AbstractEnv): +class CartPoleEnv: """Sub-class of AbstractEnv, dedicated to the cart pole scene. See the class AbstractEnv for arguments and methods. """ #Setting a default configuration - path = path = os.path.dirname(os.path.abspath(__file__)) + path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 4 DEFAULT_CONFIG = {"scene": "CartPole", "deterministic": True, "source": [0, 0, 160], "target": [0, 0, 0], - "goalList": [[0]], + "goal": False, "start_node": None, "scale_factor": 10, "dt": 0.001, @@ -38,16 +40,31 @@ class CartPoleEnv(AbstractEnv): "zFar": 4000, "time_before_start": 0, "seed": None, + "nb_actions": 2, + "dim_state": dim_state, "init_x": 0, - "max_move": 24, + "x_threshold": 100, + "max_move": 12, + "max_angle": 0.418, + "randomize_states": True, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config=None): - super().__init__(config) + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - self.x_threshold = 100 - self.theta_threshold_radians = self.config["max_move"] * math.pi / 180 - self.config.update({'max_angle': self.theta_threshold_radians}) + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + + self.x_threshold = self.env.config["x_threshold"] + self.theta_threshold_radians = self.env.config["max_move"] * math.pi / 180 + self.env.config.update({'max_angle': self.theta_threshold_radians}) high = np.array( [ @@ -59,38 +76,56 @@ def __init__(self, config=None): dtype=np.float32, ) - nb_actions = 2 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) - self.observation_space = spaces.Box(-high, high, dtype=np.float32) + self.env.observation_space = spaces.Box(-high, high, dtype=np.float32) - def reset(self): - """Reset simulation. + if self.env.root is None and not self.use_server: + self.env.init_root() + + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) + def initialize_states(self): + if self.env.config["randomize_states"]: + self.init_states = self.randomize_init_states() + self.env.config.update({'init_states': list(self.init_states)}) + else: + self.init_states = self.env.config["init_states"] + + def randomize_init_states(self): + """Randomize initial states. + + Returns: + ------- + init_states: list + List of random initial states for the environment. + Note: ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - + This method should be implemented according to needed random initialization. """ - super().reset() + init_states = self.env.np_random.uniform(low=-0.05, high=0.05, size=(self.env.config["dim_state"],)) - self.config.update({'goalPos': self.goal}) - init_states = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) - self.config.update({'init_states': list(init_states)}) - obs = start_scene(self.config, self.nb_actions) - return np.array(obs['observation']) + return init_states - def get_available_actions(self): - """Gives the actions available in the environment. - - Parameters: - ---------- - None. - - Returns: - ------- - list of the action available in the environment. + def reset(self): + """Reset simulation. """ - return self.action_space + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + + self.env.reset() + + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/CartPole/CartPoleScene.py b/sofagym/envs/CartPole/CartPoleScene.py index 7d5a58b..bc6773d 100644 --- a/sofagym/envs/CartPole/CartPoleScene.py +++ b/sofagym/envs/CartPole/CartPoleScene.py @@ -4,7 +4,7 @@ sys.path.insert(0, str(pathlib.Path(__file__).parent.absolute())+"/../") sys.path.insert(0, str(pathlib.Path(__file__).parent.absolute())) -from CartPoleToolbox import ApplyAction, GoalSetter, RewardShaper, StateInitializer +from CartPoleToolbox import ApplyAction, RewardShaper, StateInitializer from sofagym.header import addVisu from splib3.animation import AnimationManagerController from stlib3.physics.rigid import Floor @@ -48,16 +48,16 @@ def addRigidObject(node, filename, collisionFilename=None, position=[0, 0, 0, 0, return object -def createScene(root, +def createScene(root, config={"source": [0, 0, 160], "target": [0, 0, 0], - "goalPos": None, "seed": None, "zFar":4000, "init_x": 0, "max_move": 24, "max_angle": 0.418, - "dt": 0.01}, + "dt": 0.01, + "init_states": [0]*4}, mode='simu_and_visu'): # Choose the mode: visualization or computations (or both) @@ -167,5 +167,4 @@ def createScene(root, # SofaGym Env Components root.addObject(StateInitializer(name="StateInitializer", rootNode=root, pole_length=pole_length, init_states=config['init_states'])) root.addObject(RewardShaper(name="Reward", rootNode=root, max_angle=config['max_angle'], pole_length=pole_length)) - root.addObject(GoalSetter(name="GoalSetter")) root.addObject(ApplyAction(name="ApplyAction", root=root)) diff --git a/sofagym/envs/CartPole/CartPoleToolbox.py b/sofagym/envs/CartPole/CartPoleToolbox.py index c323dc6..dac1582 100644 --- a/sofagym/envs/CartPole/CartPoleToolbox.py +++ b/sofagym/envs/CartPole/CartPoleToolbox.py @@ -52,13 +52,13 @@ def __init__(self, *args, **kwargs): if kwargs["init_states"] is not None: self.init_states = kwargs["init_states"] else: - print(">> ERROR: no inital states given.") + print(">> ERROR: no initial states given.") exit(1) self.cart = self.rootNode.Modeling.Cart self.pole = self.rootNode.Modeling.Pole - def init_state(self): + def init_state(self, init_states): """Randomly initialize the environment state. Parameters: @@ -70,6 +70,7 @@ def init_state(self): None. """ + self.init_states = init_states cart_pos, cart_vel, pole_theta, pole_theta_dot = self.init_states with self.cart.MechanicalObject.position.writeable() as position: @@ -124,14 +125,11 @@ def __init__(self, *args, **kwargs): self.rootNode = None if kwargs["rootNode"]: self.rootNode = kwargs["rootNode"] - if kwargs["max_angle"]: + if kwargs["max_angle"] is not None: self.max_angle = kwargs["max_angle"] - else: - print(">> ERROR: give a max angle for the normalization of the reward.") - exit(1) - if kwargs["pole_length"]: + if kwargs["pole_length"] is not None: self.pole_length = kwargs["pole_length"] - + self.cart = self.rootNode.Modeling.Cart self.pole = self.rootNode.Modeling.Pole @@ -147,12 +145,11 @@ def getReward(self): The reward and the cost. """ - cart_pos = self._getCartPos() - pole_theta, pole_theta_dot = self.calculatePoleTheta(cart_pos) + pole_theta = self.pole.MechanicalObject.position.value.tolist()[0][5] return 1, pole_theta, self.max_angle - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -168,26 +165,6 @@ def update(self): """ pass - def _getPolePos(self): - pos = self.pole.MechanicalObject.position.value.tolist()[0] - return pos[0], pos[1] - - def _getCartPos(self): - pos = self.cart.MechanicalObject.position.value.tolist()[0][0] - return pos - - def calculatePoleTheta(self, cart_pos): - x_pos, y_pos = self._getPolePos() - sin_theta = (y_pos/self.pole_length) - theta = abs((90*math.pi/180) - math.asin(sin_theta)) - - if x_pos < cart_pos: - theta = -theta - - theta_dot = self.pole.MechanicalObject.velocity.value.tolist()[0][5] - - return theta, theta_dot - def getState(rootNode): """Compute the state of the environment/agent. @@ -203,28 +180,18 @@ def getState(rootNode): The state of the environment/agent. """ cart = rootNode.Modeling.Cart - cart_pos = cart.MechanicalObject.position.value.tolist()[0][0] cart_vel = cart.MechanicalObject.velocity.value.tolist()[0][0] - pole_theta, pole_theta_dot = rootNode.Reward.calculatePoleTheta(cart_pos) + pole = rootNode.Modeling.Pole + pole_theta = pole.MechanicalObject.position.value.tolist()[0][5] + pole_theta_dot = pole.MechanicalObject.velocity.value.tolist()[0][5] state = [cart_pos, cart_vel, pole_theta, pole_theta_dot] return state -class GoalSetter(Sofa.Core.Controller): - def __init__(self, *args, **kwargs): - Sofa.Core.Controller.__init__(self, *args, **kwargs) - - def update(self): - pass - - def set_mo_pos(self, goal): - pass - - def getReward(rootNode): reward, theta, max_angle = rootNode.Reward.getReward() done = (theta > max_angle) or (theta < -max_angle) @@ -330,6 +297,7 @@ def startCmd_CartPole(rootNode, incr, duration): """ # Definition of the elements of the animation + def executeAnimation(rootNode, incr, factor): rootNode.ApplyAction.apply_action(incr) diff --git a/sofagym/envs/CartStem/CartStemEnv.py b/sofagym/envs/CartStem/CartStemEnv.py index e46a089..ece07f8 100644 --- a/sofagym/envs/CartStem/CartStemEnv.py +++ b/sofagym/envs/CartStem/CartStemEnv.py @@ -8,26 +8,29 @@ __copyright__ = "(c) 2021, Inria" __date__ = "Feb 3 2021" -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces -import os +import os, sys import numpy as np -class CartStemEnv(AbstractEnv): +from typing import Optional + +class CartStemEnv: """Sub-class of AbstractEnv, dedicated to the gripper scene. See the class AbstractEnv for arguments and methods. """ #Setting a default configuration - path = path = os.path.dirname(os.path.abspath(__file__)) + path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 4 DEFAULT_CONFIG = {"scene": "CartStem", "deterministic": True, "source": [0, -70, 10], "target": [0, 0, 10], - "goalList": [[7, 0, 20]], + "goal": False, "start_node": None, "scale_factor": 10, "dt": 0.01, @@ -41,63 +44,71 @@ class CartStemEnv(AbstractEnv): "planning": False, "discrete": False, "start_from_history": None, - "python_version": "python3.9", + "python_version": sys.version, "zFar": 4000, "time_before_start": 0, "seed": None, "init_x": 0, "max_move": 40, + "nb_actions": 2, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config = None): - super().__init__(config) - nb_actions = 2 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) + + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) + + low_coordinates = np.array([-100]*self.env.dim_state) + high_coordinates = np.array([100]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) + + if self.env.root is None and not self.use_server: + self.env.init_root() - dim_state = 4 - low_coordinates = np.array([-100]*dim_state) - high_coordinates = np.array([100]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) + + def initialize_states(self): + self.env.initialize_states() + self.env.config.update({'init_x': -(self.env.config["max_move"]/8) + (self.env.config["max_move"]/4)*self.env.np_random.random()}) def step(self, action): - obs, reward, done, info = super().step(action) - if abs(obs[0]) > self.config["max_move"]: + obs, reward, done, info = self.env.step(action) + if abs(obs[0]) > self.env.config["max_move"]: done = True return obs, reward, done, info def reset(self): """Reset simulation. - - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - """ - super().reset() - - self.config.update({'init_x': -(self.config["max_move"]/8) + (self.config["max_move"]/4)*np.random.random()}) - self.config.update({'goalPos': self.goal}) + self.initialize_states() - obs = start_scene(self.config, self.nb_actions) - - return np.array(obs['observation']) + if self.env.config["goal"]: + self.init_goal() - def get_available_actions(self): - """Gives the actions available in the environment. - - Parameters: - ---------- - None. - - Returns: - ------- - list of the action available in the environment. - """ - return self.action_space + self.env.reset() + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + return state diff --git a/sofagym/envs/CartStem/CartStemScene.py b/sofagym/envs/CartStem/CartStemScene.py index a6a9fac..4cb3778 100644 --- a/sofagym/envs/CartStem/CartStemScene.py +++ b/sofagym/envs/CartStem/CartStemScene.py @@ -23,20 +23,11 @@ from sofagym.header import addVisu as visu from CartStem import CartStem -from CartStemToolbox import rewardShaper, sceneModerator, applyAction, goalSetter - - -def add_goal_node(root, pos): - goal = root.addChild("Goal") - goal.addObject('VisualStyle', displayFlags="showCollisionModels") - goal_mo = goal.addObject('MechanicalObject', name='GoalMO', showObject=True, drawMode="1", showObjectScale=0.5, - showColor=[1, 0, 0, 0.5], position= pos) - return goal_mo +from CartStemToolbox import rewardShaper, sceneModerator, applyAction def createScene(rootNode, config={"source": [0, -70, 10], "target": [0, 0, 10], - "goalPos": [7, 0, 20], "seed": None, "zFar":4000, "init_x": 0, @@ -62,7 +53,6 @@ def createScene(rootNode, config={"source": [0, -70, 10], cartstem.onEnd(rootNode) cartstem.cart.addObject('ConstantForceField', totalForce=[0, 0, 0, 0, 0, 0]) - rootNode.addObject(goalSetter(name="GoalSetter")) rootNode.addObject(rewardShaper(name="Reward", rootNode=rootNode, max_dist= cartstem_config['max_move'])) rootNode.addObject(sceneModerator(name="sceneModerator", cartstem = cartstem)) rootNode.addObject(applyAction(name="applyAction", root= rootNode, cartstem=cartstem)) diff --git a/sofagym/envs/CartStem/CartStemToolbox.py b/sofagym/envs/CartStem/CartStemToolbox.py index 31c947e..01fd3f5 100644 --- a/sofagym/envs/CartStem/CartStemToolbox.py +++ b/sofagym/envs/CartStem/CartStemToolbox.py @@ -86,7 +86,7 @@ def getReward(self): dist = abs(sphere_pos-cart_pos) return 1, dist - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -151,17 +151,6 @@ def getState(rootNode): return state -class goalSetter(Sofa.Core.Controller): - def __init__(self, *args, **kwargs): - Sofa.Core.Controller.__init__(self, *args, **kwargs) - - def update(self): - pass - - def set_mo_pos(self, goal): - pass - - def getReward(rootNode): r, dist = rootNode.Reward.getReward() done = dist > 5 diff --git a/sofagym/envs/CartStemContact/CartStemContactEnv.py b/sofagym/envs/CartStemContact/CartStemContactEnv.py index 9c8323b..3eebd57 100644 --- a/sofagym/envs/CartStemContact/CartStemContactEnv.py +++ b/sofagym/envs/CartStemContact/CartStemContactEnv.py @@ -8,27 +8,31 @@ __copyright__ = "(c) 2021, Inria" __date__ = "Feb 3 2021" -import os +import os, sys -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces import numpy as np -class CartStemContactEnv(AbstractEnv): +from typing import Optional + +class CartStemContactEnv: """Sub-class of AbstractEnv, dedicated to the gripper scene. See the class AbstractEnv for arguments and methods. """ #Setting a default configuration - path = os.path.dirname(os.path.abspath(__file__)) + path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 8 DEFAULT_CONFIG = {"scene": "CartStemContact", "deterministic": True, "source": [0, -50, 10], "target": [0, 0, 10], + "goal": True, "goalList": [[7, 0, 20]], "start_node": None, "scale_factor": 30, @@ -43,72 +47,77 @@ class CartStemContactEnv(AbstractEnv): "planning": False, "discrete": False, "start_from_history": None, - "python_version": "python3.9", + "python_version": sys.version, "zFar": 4000, "time_before_start": 0, "seed": None, "init_x": 5, "cube_x": [-6, 6], "max_move": 7.5, + "nb_actions": -1, + "dim_state": dim_state, + "init_states": [0] * dim_state, + "randomize_states": False, + "use_server": False } + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - def __init__(self, config = None): - super().__init__(config) - nb_actions = -1 - low = np.array([-1]*1) - high = np.array([1]*1) - self.action_space = spaces.Box(low=low, high=high, shape=(1,), dtype='float32') - self.nb_actions = str(nb_actions) + self.initialize_states() - dim_state = 8 + if self.env.config["goal"]: + self.init_goal() - low_coordinates = np.array([-1]*dim_state) - high_coordinates = np.array([1]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') + low = np.array([-1]*1) + high = np.array([1]*1) + self.env.action_space = spaces.Box(low=low, high=high, shape=(1,), dtype=np.float32) + self.nb_actions = str(self.env.nb_actions) + + low_coordinates = np.array([-1]*self.env.dim_state) + high_coordinates = np.array([1]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) + + if self.env.root is None and not self.use_server: + self.env.init_root() + + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) + + def initialize_states(self): + self.env.initialize_states() + + low_cube, high_cube = -6+ 2*self.env.np_random.random(), 6 - 2*self.env.np_random.random() + self.env.config.update({'cube_x': [low_cube, high_cube]}) + self.env.config.update({'init_x': (low_cube + 3) + (high_cube-low_cube-3)*self.env.np_random.random()}) + + if self.env.np_random.random() > 0.5: + x_goal = low_cube + 3.5*self.env.np_random.random() + else: + x_goal = high_cube - 3.5*self.env.np_random.random() - def step(self, action): - return super().step(action) + self.env.config.update({'goalList': [[x_goal, 0, 20]]}) + self.env.config.update({'max_move': max(abs(low_cube-1), high_cube+1)}) def reset(self): """Reset simulation. - - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - """ - low_cube, high_cube = -6+ 2*np.random.random(), 6 - 2*np.random.random() - self.config.update({'cube_x': [low_cube, high_cube]}) - self.config.update({'init_x': (low_cube + 3) + (high_cube-low_cube-3)*np.random.random()}) + self.initialize_states() - if np.random.random() > 0.5: - x_goal = low_cube + 3.5*np.random.random() - else: - x_goal = high_cube - 3.5*np.random.random() - self.config.update({'goalList': [[x_goal, 0, 20]]}) - self.goalList = self.config["goalList"] - super().reset() - self.config.update({'max_move': max(abs(low_cube-1), high_cube+1)}) - self.config.update({'goalPos': self.goal}) - - obs = start_scene(self.config, self.nb_actions) - - return np.array(obs['observation']) - - def get_available_actions(self): - """Gives the actions available in the environment. - - Parameters: - ---------- - None. - - Returns: - ------- - list of the action available in the environment. - """ - return self.action_space + if self.env.config["goal"]: + self.init_goal() + self.env.reset() + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/CartStemContact/CartStemContactToolbox.py b/sofagym/envs/CartStemContact/CartStemContactToolbox.py index 462f8ee..1bd5d49 100644 --- a/sofagym/envs/CartStemContact/CartStemContactToolbox.py +++ b/sofagym/envs/CartStemContact/CartStemContactToolbox.py @@ -89,7 +89,7 @@ def getReward(self): return reward, dist - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -103,6 +103,7 @@ def update(self): None. """ + self.goal = goal current_sphere_pos = self._getSpherePos() self.init_goal_dist = float(abs(current_sphere_pos[0]-self.goal[0])) @@ -119,8 +120,8 @@ def __init__(self, *args, **kwargs): if 'goalPos' in kwargs: self.goalPos = kwargs["goalPos"] - def update(self): - pass + def update(self, goal): + self.goalPos = goal def set_mo_pos(self, goal): pass diff --git a/sofagym/envs/CatchTheObject/CatchTheObjectEnv.py b/sofagym/envs/CatchTheObject/CatchTheObjectEnv.py index aecb624..ef6bbc3 100644 --- a/sofagym/envs/CatchTheObject/CatchTheObjectEnv.py +++ b/sofagym/envs/CatchTheObject/CatchTheObjectEnv.py @@ -8,16 +8,18 @@ __copyright__ = "(c) 2021, Inria" __date__ = "Feb 3 2021" -import os +import os, sys -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces import numpy as np -class CatchTheObject(AbstractEnv): +from typing import Optional + +class CatchTheObject: """Sub-class of AbstractEnv, dedicated to the gripper scene. See the class AbstractEnv for arguments and methods. @@ -25,10 +27,12 @@ class CatchTheObject(AbstractEnv): #Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 5 DEFAULT_CONFIG = {"scene": "CatchTheObject", "deterministic": True, "source": [0, -70, 10], "target": [0, 0, 10], + "goal": True, "goalList": [[7, 0, 20]], "start_node": None, "scale_factor": 10, @@ -43,63 +47,61 @@ class CatchTheObject(AbstractEnv): "planning": False, "discrete": False, "start_from_history": None, - "python_version": "python3.9", + "python_version": sys.version, "zFar": 4000, "time_before_start": 0, "seed": None, "max_move": 10, - "max_pressure": 15 + "max_pressure": 15, + "nb_actions": -1, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config = None): - super().__init__(config) - nb_actions = -1 - low = np.array([-1]*1) - high = np.array([1]*1) - self.action_space = spaces.Box(low=low, high=high, shape=(1,), dtype='float32') - self.nb_actions = str(nb_actions) + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - dim_state = 5 - low_coordinates = np.array([-1]*dim_state) - high_coordinates = np.array([1]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') + self.initialize_states() - def step(self, action): - return super().step(action) - - def reset(self): - """Reset simulation. - - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - - """ + if self.env.config["goal"]: + self.init_goal() - super().reset() - - self.count = 0 - self.config.update({'goalPos': self.goal}) - # obs = super().reset() - # return np.array(obs) - obs = start_scene(self.config, self.nb_actions) + low = np.array([-1]*1) + high = np.array([1]*1) + self.env.action_space = spaces.Box(low=low, high=high, shape=(1,), dtype=np.float32) + self.nb_actions = str(self.env.nb_actions) - return np.array(obs['observation']) + low_coordinates = np.array([-1]*self.env.dim_state) + high_coordinates = np.array([1]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - def get_available_actions(self): - """Gives the actions available in the environment. + if self.env.root is None and not self.use_server: + self.env.init_root() - Parameters: - ---------- - None. + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) - Returns: - ------- - list of the action available in the environment. + def reset(self): + """Reset simulation. """ - return self.action_space - - - + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + + self.env.reset() + + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/CatchTheObject/CatchTheObjectToolbox.py b/sofagym/envs/CatchTheObject/CatchTheObjectToolbox.py index 7087b65..7c0f8b6 100644 --- a/sofagym/envs/CatchTheObject/CatchTheObjectToolbox.py +++ b/sofagym/envs/CatchTheObject/CatchTheObjectToolbox.py @@ -88,7 +88,7 @@ def getReward(self): return r, dist, under - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -102,6 +102,7 @@ def update(self): None. """ + pos_goal = goal self.max_dist = np.linalg.norm(np.array([0, self.ball.max_high])-np.array([self.cart.max_move, 0])) @@ -113,8 +114,8 @@ def __init__(self, *args, **kwargs): if 'goalPos' in kwargs: self.goalPos = kwargs["goalPos"] - def update(self): - pass + def update(self, goal): + self.goalPos = goal def set_mo_pos(self, goal): pass diff --git a/sofagym/envs/CatheterBeam/CatheterBeamEnv.py b/sofagym/envs/CatheterBeam/CatheterBeamEnv.py index 54729e9..1400b14 100644 --- a/sofagym/envs/CatheterBeam/CatheterBeamEnv.py +++ b/sofagym/envs/CatheterBeam/CatheterBeamEnv.py @@ -1,21 +1,23 @@ import os import sys +from typing import Optional import numpy as np from gym import spaces -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene -class CatheterBeamEnv(AbstractEnv): +class CatheterBeamEnv: """Sub-class of AbstractEnv, dedicated to the catheter beam scene. See the class AbstractEnv for arguments and methods. """ #Setting a default configuration - path = path = os.path.dirname(os.path.abspath(__file__)) + path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 12 DEFAULT_CONFIG = {"scene": "CatheterBeam", "deterministic": True, "source": [-1169.51, 298.574, 257.631], @@ -40,46 +42,55 @@ class CatheterBeamEnv(AbstractEnv): "scale": 30, "rotation": [140.0, 0.0, 0.0], "translation": [0.0, 0.0, 0.0], - "goalList": [1226, 1663, 1797, 1544, 2233, 2580, 3214] + "goal": True, + "goalList": [1226, 1663, 1797, 1544, 2233, 2580, 3214], + "nb_actions": 12, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config = None): - super().__init__(config) - nb_actions = 12 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) + + self.initialize_states() - dim_state = 12 - low_coordinates = np.array([-1]*dim_state) - high_coordinates = np.array([1]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) + if self.env.config["goal"]: + self.init_goal() - def reset(self): - """Reset simulation. + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. + low_coordinates = np.array([-1]*self.env.dim_state) + high_coordinates = np.array([1]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - """ - super().reset() + if self.env.root is None and not self.use_server: + self.env.init_root() - self.config.update({'goalPos': self.goal}) + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) - obs = start_scene(self.config, self.nb_actions) - - return np.array(obs['observation']) + def reset(self): + """Reset simulation. + """ + self.initialize_states() - def get_available_actions(self): - """Gives the actions available in the environment. + if self.env.config["goal"]: + self.init_goal() - Parameters: - ---------- - None. + self.env.reset() - Returns: - ------- - list of the action available in the environment. - """ - return self.action_space + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/CatheterBeam/CatheterBeamToolbox.py b/sofagym/envs/CatheterBeam/CatheterBeamToolbox.py index d50bd20..d985a28 100644 --- a/sofagym/envs/CatheterBeam/CatheterBeamToolbox.py +++ b/sofagym/envs/CatheterBeam/CatheterBeamToolbox.py @@ -77,7 +77,7 @@ def getReward(self): return -current_dist, current_dist - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -91,6 +91,7 @@ def update(self): None. """ + self.goal_pos = self.root.CollisionModel.DOFs1.position.value[goal][:3] tip = self.root.InstrumentCombined.DOFs.position[-1][:3] self.init_dist = np.linalg.norm(np.array(tip)-np.array(self.goal_pos)) self.prev_dist = self.init_dist @@ -137,7 +138,7 @@ def __init__(self, *args, **kwargs): if kwargs["goalPos"]: self.goalPos = kwargs["goalPos"] - def update(self): + def update(self, goal): """Set the position of the goal. This function is used as an initialization function. @@ -151,6 +152,7 @@ def update(self): None. """ + self.goalPos = goal new_position = self.rootNode.CollisionModel.DOFs1.position.value[self.goalPos][:3] with self.goal.GoalMO.position.writeable() as position: position[0] = new_position diff --git a/sofagym/envs/Diamond/DiamondEnv.py b/sofagym/envs/Diamond/DiamondEnv.py index c50adc9..86c92f1 100644 --- a/sofagym/envs/Diamond/DiamondEnv.py +++ b/sofagym/envs/Diamond/DiamondEnv.py @@ -8,9 +8,9 @@ __copyright__ = "(c) 2021, Robocath, CNRS, Inria" __date__ = "Dec 01 2021" -import os +import os, sys -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from sofagym.viewer import LegacyViewer from sofagym.envs.Diamond.DiamondToolbox import startCmd @@ -19,7 +19,9 @@ import numpy as np -class DiamondRobotEnv(AbstractEnv): +from typing import Optional + +class DiamondRobotEnv: """Sub-class of AbstractEnv, dedicated to the trunk scene. See the class AbstractEnv for arguments and methods. @@ -27,10 +29,12 @@ class DiamondRobotEnv(AbstractEnv): # Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = (6, 3) DEFAULT_CONFIG = {"scene": "Diamond", "deterministic": True, "source": [-288, -81, 147], "target": [4, -6, 52], + "goal": True, "goalList": [[30.0, 0.0, 150.0], [-30.0, 0.0, 150.0], [0.0, 30.0, 150.0], [0.0, -30.0, 150.0]], "scale_factor": 5, "timer_limit": 50, @@ -43,82 +47,61 @@ class DiamondRobotEnv(AbstractEnv): "discrete": True, "seed": 0, "start_from_history": None, - "python_version": "python3.8", + "python_version": sys.version, "zFar": 5000, - "dt": 0.01 + "dt": 0.01, + "nb_actions": 8, + "dim_state": dim_state, + "randomize_states": False, + "init_states": 0, + "use_server": False } - def __init__(self, config=None): - super().__init__(config) - nb_actions = 8 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) - - dim_state = (6, 3) - low_coordinates = np.ones(shape=dim_state)*-1 - high_coordinates = np.ones(shape=dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - def step(self, action): - if self.viewer: - self.viewer.step(action) + self.initialize_states() - return super().step(action) + if self.env.config["goal"]: + self.init_goal() - def reset(self): - """Reset simulation. - - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - - """ - super().reset() + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) - self.goal = [-30 + 60 * np.random.random(), -30 + 60 * np.random.random(), 125 + 20 * np.random.random()] + low_coordinates = np.ones(shape=self.env.dim_state)*-1 + high_coordinates = np.ones(shape=self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - self.config.update({'goalPos': self.goal}) - obs = start_scene(self.config, self.nb_actions) - if self.viewer: - self.viewer.reset() + if self.env.root is None and not self.use_server: + self.env.init_root() - return np.array(obs['observation']) + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) - - def render(self, mode='rgb_array'): - """See the current state of the environment. + def init_goal(self): + # Set a new random goal from the list + self.env.goal = [-30 + 60 * self.env.np_random.random(), -30 + 60 * self.env.np_random.random(), 125 + 20 * self.env.np_random.random()] + self.env.config.update({'goalPos': self.env.goal}) - Get the OpenGL Context to render an image (snapshot) of the simulation - state. - - Parameters: - ---------- - mode: string, default = 'rgb_array' - Type of representation. - - Returns: - ------- - None. + def reset(self): + """Reset simulation. """ - if not self.viewer: - display_size = self.config["display_size"] # Sim display - self.viewer = LegacyViewer(self, display_size, startCmd=startCmd) - - # Use the viewer to display the environment. - self.viewer.render() - + self.initialize_states() - def get_available_actions(self): - """Gives the actions available in the environment. + if self.env.config["goal"]: + self.init_goal() - Parameters: - ---------- - None. + self.env.reset() - Returns: - ------- - list of the action available in the environment. - """ - return list(range(int(self.nb_actions))) + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/Diamond/DiamondToolbox.py b/sofagym/envs/Diamond/DiamondToolbox.py index 8710078..7147706 100644 --- a/sofagym/envs/Diamond/DiamondToolbox.py +++ b/sofagym/envs/Diamond/DiamondToolbox.py @@ -83,7 +83,7 @@ def getReward(self): return min(3*reward**(1/2), 1.0), current_dist - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -97,7 +97,7 @@ def update(self): None. """ - + self.goal_pos = goal tip = self.rootNode.Robot.Actuators.actuatedPoints.position[0] self.init_dist = np.linalg.norm(np.array(tip)-np.array(self.goal_pos)) self.prev_dist = self.init_dist @@ -145,7 +145,7 @@ def __init__(self, *args, **kwargs): if kwargs["goalPos"]: self.goalPos = kwargs["goalPos"] - def update(self): + def update(self, goal): """Set the position of the goal. This function is used as an initialization function. @@ -159,6 +159,7 @@ def update(self): None. """ + self.goalPos = goal with self.goalMO.position.writeable() as position: position[0] = self.goalPos @@ -346,7 +347,7 @@ def getPos(root): _: list The position(s) of the object(s) of the scene. """ - return + return root.Robot.tetras.position.value.tolist() def setPos(root, pos): @@ -368,4 +369,4 @@ def setPos(root, pos): Don't forget to init the new value of the position. """ - return + root.Robot.tetras.position.value = np.array(pos) diff --git a/sofagym/envs/Gripper/GripperEnv.py b/sofagym/envs/Gripper/GripperEnv.py index 10e7c41..beb18cc 100644 --- a/sofagym/envs/Gripper/GripperEnv.py +++ b/sofagym/envs/Gripper/GripperEnv.py @@ -8,15 +8,17 @@ __copyright__ = "(c) 2020, Inria" __date__ = "Oct 7 2020" -import os +import os, sys import numpy as np -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces -class GripperEnv(AbstractEnv): +from typing import Optional + +class GripperEnv: """Sub-class of AbstractEnv, dedicated to the gripper scene. See the class AbstractEnv for arguments and methods. @@ -24,10 +26,12 @@ class GripperEnv(AbstractEnv): # Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 31 DEFAULT_CONFIG = {"scene": "Gripper", "deterministic": True, "source": [0, -80, 350], "target": [0, -80, 0], + "goal": True, "goalList": [[0, 10, 0], [0, 20, 0], [0, 30, 0]], "start_node": None, "scale_factor": 5, @@ -42,52 +46,55 @@ class GripperEnv(AbstractEnv): "discrete": True, "seed": None, "start_from_history": None, - "python_version": "python3", - "dt": 0.01 + "python_version": sys.version, + "dt": 0.01, + "nb_actions": 8, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config=None): - super().__init__(config) - nb_actions = 8 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) - - dim_state = 31 - low_coordinates = np.array([-1]*dim_state) - high_coordinates = np.array([1]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - - - def step(self, action): - return super().step(action) + self.initialize_states() - def reset(self): - """Reset simulation. + if self.env.config["goal"]: + self.init_goal() - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - - """ - super().reset() + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) - self.config.update({'goalPos': self.goal}) - obs = start_scene(self.config, self.nb_actions) + low_coordinates = np.array([-1]*self.env.dim_state) + high_coordinates = np.array([1]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - return (np.array(obs['observation'])) + if self.env.root is None and not self.use_server: + self.env.init_root() - def get_available_actions(self): - """Gives the actions available in the environment. + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) + + def reset(self): + """Reset simulation. + """ + self.initialize_states() - Parameters: - ---------- - None. + if self.env.config["goal"]: + self.init_goal() - Returns: - ------- - list of the action available in the environment. - """ - return list(range(int(self.nb_actions))) + self.env.reset() + + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/Gripper/GripperTools.py b/sofagym/envs/Gripper/GripperTools.py index a1a6aa9..fec68e5 100644 --- a/sofagym/envs/Gripper/GripperTools.py +++ b/sofagym/envs/Gripper/GripperTools.py @@ -89,7 +89,7 @@ def getReward(self): self.cost = current_cost return round(reward, 3)/20, self.cost - def update(self): + def update(self, goal=None): """Compute the distance between object and goal. This function is used as an initialization function. @@ -103,6 +103,7 @@ def update(self): None. """ + self.goal_pos = goal self.cost = abs(self.effMO.position[0][1] - self.goal_pos[1]) @@ -145,7 +146,7 @@ def __init__(self, *args, **kwargs): if kwargs["goalPos"]: self.goalPos = kwargs["goalPos"] - def update(self): + def update(self, goal): """Set the position of the goal. This function is used as an initialization function. @@ -159,6 +160,7 @@ def update(self): None. """ + self.goalPos = goal with self.goalMO.position.writeable() as position: position += self.goalPos diff --git a/sofagym/envs/Maze/MazeEnv.py b/sofagym/envs/Maze/MazeEnv.py index 0c804a6..0d8df3a 100644 --- a/sofagym/envs/Maze/MazeEnv.py +++ b/sofagym/envs/Maze/MazeEnv.py @@ -8,15 +8,17 @@ __copyright__ = "(c) 2020, Inria" __date__ = "Oct 7 2020" -import os +import os, sys import numpy as np -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces -class MazeEnv(AbstractEnv): +from typing import Optional + +class MazeEnv: """Sub-class of AbstractEnv, dedicated to the trunk scene. See the class AbstractEnv for arguments and methods. @@ -24,10 +26,12 @@ class MazeEnv(AbstractEnv): # Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 9 DEFAULT_CONFIG = {"scene": "Maze", "deterministic": True, "source": [-82.0819, 186.518, 135.963], "target": [-2.09447, 5.75347, -4.34572], + "goal": True, "goalList": [334, 317, 312, 301], "goal_node": 270, "start_node": 269, @@ -43,51 +47,57 @@ class MazeEnv(AbstractEnv): "discrete": True, "seed": 0, "start_from_history": None, - "python_version": "python3.8", + "python_version": sys.version, "zFar": 1000, "dt": 0.01, "time_before_start": 20, + "nb_actions": 6, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config=None): - super().__init__(config) - nb_actions = 6 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - dim_state = 9 - low_coordinates = np.array([-1]*dim_state) - high_coordinates = np.array([1]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype='float32') + self.initialize_states() - def step(self, action): - return super().step(action) + if self.env.config["goal"]: + self.init_goal() - def reset(self): - """Reset simulation. + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - - """ - super().reset() + low_coordinates = np.array([-1]*self.env.dim_state) + high_coordinates = np.array([1]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - self.config.update({'goalPos': self.goal}) - obs = start_scene(self.config, self.nb_actions) + if self.env.root is None and not self.use_server: + self.env.init_root() - return np.array(obs['observation']) + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) - def get_available_actions(self): - """Gives the actions available in the environment. - - Parameters: - ---------- - None. - - Returns: - ------- - list of the action available in the environment. + def reset(self): + """Reset simulation. """ - return list(range(int(self.nb_actions))) \ No newline at end of file + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + + self.env.reset() + + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/Maze/MazeScene.py b/sofagym/envs/Maze/MazeScene.py index d26b20e..ca44c8b 100644 --- a/sofagym/envs/Maze/MazeScene.py +++ b/sofagym/envs/Maze/MazeScene.py @@ -22,6 +22,7 @@ from MazeToolbox import goalSetter, rewardShaper from Sphere import Sphere from splib3.animation import animate +from splib3.animation import AnimationManagerController from splib3.numerics import RigidDof from stlib3.scene import ContactHeader, Scene from tripod import Tripod @@ -52,7 +53,7 @@ def createScene(rootNode, config={"source": [0, 300, 0], "target": [0, 0, 0], "goalList": [0, 0, 0], "goal_node": 0, - "goalPos": [0.0, 0.0, 0.0], + "goalPos": 0, "dt": 0.01}, mode='simu_and_visu'): pluginList = ["ArticulatedSystemPlugin", @@ -115,6 +116,8 @@ def createScene(rootNode, config={"source": [0, 300, 0], scene.Settings.mouseButton.stiffness = 10 scene.Simulation.TimeIntegrationSchema.rayleighStiffness = 0.05 ContactHeader(rootNode, alarmDistance=0.5, contactDistance=0.2, frictionCoef=0.2) + + rootNode.addObject(AnimationManagerController(rootNode, name="AnimationManager")) # Visu if visu: diff --git a/sofagym/envs/Maze/MazeToolbox.py b/sofagym/envs/Maze/MazeToolbox.py index e4e7656..bfa0ac3 100644 --- a/sofagym/envs/Maze/MazeToolbox.py +++ b/sofagym/envs/Maze/MazeToolbox.py @@ -110,7 +110,7 @@ def getReward(self): else: return new_ratio, None - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -124,7 +124,7 @@ def update(self): None. """ - + self.goal_node = goal edges = [] with self.path_mesh.edges.writeable() as Topoedges: for edge in Topoedges: @@ -188,7 +188,7 @@ def __init__(self, *args, **kwargs): if kwargs["goalPos"]: self.goalPos = kwargs["goalPos"] - def update(self): + def update(self, goal): """Set the position of the goal. This function is used as an initialization function. @@ -202,6 +202,7 @@ def update(self): None. """ + self.goalPos = goal new_position = self.rootNode.Modelling.Tripod.RigidifiedStructure.FreeCenter.Maze.Path.dofs.position.value[self.goalPos][:3] with self.goal.GoalMO.position.writeable() as position: position[0] = new_position diff --git a/sofagym/envs/SimpleMaze/SimpleMazeEnv.py b/sofagym/envs/SimpleMaze/SimpleMazeEnv.py index 9d2d461..4af6110 100644 --- a/sofagym/envs/SimpleMaze/SimpleMazeEnv.py +++ b/sofagym/envs/SimpleMaze/SimpleMazeEnv.py @@ -8,15 +8,17 @@ __copyright__ = "(c) 2021, Robocath, CNRS, Inria" __date__ = "Mar 23 2021" -import os +import os, sys import numpy as np -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces -class SimpleMazeEnv(AbstractEnv): +from typing import Optional + +class SimpleMazeEnv: """Sub-class of AbstractEnv, dedicated to the trunk scene. See the class AbstractEnv for arguments and methods. @@ -24,14 +26,16 @@ class SimpleMazeEnv(AbstractEnv): # Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 13 DEFAULT_CONFIG = {"scene": "SimpleMaze", "deterministic": True, "source": [0, 200, 0], "target": [0, 0, 0], + "goal": True, "goalList": [301, 334, 317, 312], "goal_node": 334, "start_node": 269, - "scale_factor": 200, + "scale_factor": 10, "timer_limit": 50, "timeout": 30, "display_size": (1600, 800), @@ -42,41 +46,59 @@ class SimpleMazeEnv(AbstractEnv): "discrete": True, "seed": 0, "start_from_history": None, - "python_version": "python3.9", + "python_version": sys.version, "zFar": 5000, - "dt": 0.01 + "dt": 0.01, + "nb_actions": 4, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config=None): - super().__init__(config) - nb_actions = 4 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - dim_state = 13 - low_coordinates = np.array([-1]*dim_state) - high_coordinates = np.array([1]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') + self.initialize_states() - def step(self, action): - return super().step(action) + if self.env.config["goal"]: + self.init_goal() - def reset(self): - """Reset simulation. + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. + low_coordinates = np.array([-1]*self.env.dim_state) + high_coordinates = np.array([1]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - """ - super().reset() + if self.env.root is None and not self.use_server: + self.env.init_root() - self.config.update({'goalPos': self.goal}) - obs = start_scene(self.config, self.nb_actions) + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) - return np.array(obs['observation']) + def reset(self): + """Reset simulation. + """ + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + + self.env.reset() + + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state def get_available_actions(self): """Gives the actions available in the environment. @@ -90,15 +112,13 @@ def get_available_actions(self): list of the action available in the environment. """ if self.nb_actions == "4": - if not self.past_actions: + if not self.env.past_actions: return [0, 1, 2, 3] - last_action = self.past_actions[-1] + last_action = self.env.past_actions[-1] print(last_action) available_actions = [[0, 1, 2], [0, 1, 3], [0, 2, 3], [1, 2, 3]] return available_actions[last_action] - return list(range(int(self.nb_actions))) - - + return self.env.action_space diff --git a/sofagym/envs/SimpleMaze/SimpleMazeScene.py b/sofagym/envs/SimpleMaze/SimpleMazeScene.py index 537c354..50a2b97 100644 --- a/sofagym/envs/SimpleMaze/SimpleMazeScene.py +++ b/sofagym/envs/SimpleMaze/SimpleMazeScene.py @@ -37,7 +37,7 @@ def add_goal_node(root): def createScene(root, config={"source": [0, 1000, 0], "target": [0, 0, 0], "goal_node": 0, - "goalPos": [0.0, 0.0, 0.0], + "goalPos": 0, "dt": 0.01}, mode='simu_and_visu'): # Chose the mode: visualization or computations (or both) @@ -92,7 +92,7 @@ def createScene(root, config={"source": [0, 1000, 0], goal = add_goal_node(root) - root.addObject(rewardShaper(name="Reward", rootNode=root, goal_node=config['goalList'][config['goal_node']], + root.addObject(rewardShaper(name="Reward", rootNode=root, goal_node=config['goalPos'], path_mesh=p_mesh, path_mo=p_mo, ball_mo=ball_mo)) root.addObject(goalSetter(name="GoalSetter", rootNode=root, goal=goal, goalPos=config['goalPos'])) diff --git a/sofagym/envs/SimpleMaze/SimpleMazeToolbox.py b/sofagym/envs/SimpleMaze/SimpleMazeToolbox.py index 4b0d233..8f4ee40 100644 --- a/sofagym/envs/SimpleMaze/SimpleMazeToolbox.py +++ b/sofagym/envs/SimpleMaze/SimpleMazeToolbox.py @@ -156,7 +156,7 @@ def getReward(self): else: return 0.0, False - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -170,7 +170,7 @@ def update(self): None. """ - + self.goal_node = goal edges = [] with self.path_mesh.edges.writeable() as Topoedges: for edge in Topoedges: @@ -233,7 +233,7 @@ def __init__(self, *args, **kwargs): if kwargs["goalPos"]: self.goalPos = kwargs["goalPos"] - def update(self): + def update(self, goal): """Set the position of the goal. This function is used as an initialization function. @@ -247,6 +247,7 @@ def update(self): None. """ + self.goalPos = goal new_position = self.rootNode.model.Maze.Path.dofs.position.value[self.goalPos][:3] with self.goal.GoalMO.position.writeable() as position: position[0] = new_position @@ -447,9 +448,6 @@ def getPos(root): _: list The position(s) of the object(s) of the scene. """ - - root.GoalSetter.update() - maze = root.model.Maze.maze_mesh_mo.position.value.tolist() spheres = root.Sphere.sphere_mo.position.value.tolist() diff --git a/sofagym/envs/StemPendulum/StemPendulumEnv.py b/sofagym/envs/StemPendulum/StemPendulumEnv.py index 99b3899..54209a8 100644 --- a/sofagym/envs/StemPendulum/StemPendulumEnv.py +++ b/sofagym/envs/StemPendulum/StemPendulumEnv.py @@ -8,27 +8,30 @@ __copyright__ = "(c) 2021, Inria" __date__ = "Feb 3 2021" -import os +import os, sys import numpy as np -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces -class StemPendulumEnv(AbstractEnv): +from typing import Optional + +class StemPendulumEnv: """Sub-class of AbstractEnv, dedicated to the gripper scene. See the class AbstractEnv for arguments and methods. """ #Setting a default configuration - path = path = os.path.dirname(os.path.abspath(__file__)) + path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 5 DEFAULT_CONFIG = {"scene": "StemPendulum", "deterministic": True, "source": [0, 0, 30], "target": [0, 0, 0], - "goalList": [[7, 0, 20]], + "goal": False, "start_node": None, "scale_factor": 10, "dt": 0.01, @@ -42,61 +45,60 @@ class StemPendulumEnv(AbstractEnv): "planning": False, "discrete": False, "start_from_history": None, - "python_version": "python3.8", + "python_version": sys.version, "zFar": 4000, "time_before_start": 0, "seed": None, "max_torque": 500, + "nb_actions": -1, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config = None): - super().__init__(config) - nb_actions = -1 + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) + + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + low = np.array([-1]*1) high = np.array([1]*1) - self.action_space = spaces.Box(low=low, high=high, shape=(1,), dtype='float32') - self.nb_actions = str(nb_actions) + self.env.action_space = spaces.Box(low=low, high=high, shape=(1,), dtype=np.float32) + self.nb_actions = str(self.env.nb_actions) + + low_coordinates = np.array([-2]*self.env.dim_state) + high_coordinates = np.array([2]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - dim_state = 5 - low_coordinates = np.array([-2]*dim_state) - high_coordinates = np.array([2]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') + if self.env.root is None and not self.use_server: + self.env.init_root() - def step(self, action): - return super().step(action) + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) def reset(self): """Reset simulation. - - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - """ - super().reset() - - self.count = 0 - self.config.update({'goalPos': self.goal}) - # obs = super().reset() - # return np.array(obs) - obs = start_scene(self.config, self.nb_actions) - - return np.array(obs['observation']) - - def get_available_actions(self): - """Gives the actions available in the environment. - - Parameters: - ---------- - None. - - Returns: - ------- - list of the action available in the environment. - """ - return self.action_space - - - + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + + self.env.reset() + + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/StemPendulum/StemPendulumScene.py b/sofagym/envs/StemPendulum/StemPendulumScene.py index bda7037..ac19a1e 100644 --- a/sofagym/envs/StemPendulum/StemPendulumScene.py +++ b/sofagym/envs/StemPendulum/StemPendulumScene.py @@ -26,20 +26,12 @@ from sofagym.utils import addRigidObject from StemPendulum import StemPendulum -from StemPendulumToolbox import rewardShaper, sceneModerator, applyAction, goalSetter +from StemPendulumToolbox import rewardShaper, sceneModerator, applyAction from Controller import ControllerStemPendulum -def add_goal_node(root, pos): - goal = root.addChild("Goal") - goal.addObject('VisualStyle', displayFlags="showCollisionModels") - goal_mo = goal.addObject('MechanicalObject', name='GoalMO', showObject=True, drawMode="1", showObjectScale=0.5, - showColor=[1, 0, 0, 0.5], position= pos) - return goal_mo - def createScene(rootNode, config = {"source": [0, 0, 30], "target": [0, 0, 0], - "goalPos": [7, 0, 20], "seed": None, "zFar":4000, "max_torque": 500, @@ -60,7 +52,6 @@ def createScene(rootNode, config = {"source": [0, 0, 30], stempendulum = StemPendulum(stempendulum_config = stempendulum_config) stempendulum.onEnd(rootNode) - rootNode.addObject(goalSetter(name="GoalSetter")) rootNode.addObject(sceneModerator(name="sceneModerator", stempendulum = stempendulum)) rootNode.addObject(rewardShaper(name="Reward", rootNode=rootNode)) rootNode.addObject(applyAction(name="applyAction", root= rootNode, stempendulum=stempendulum)) diff --git a/sofagym/envs/StemPendulum/StemPendulumToolbox.py b/sofagym/envs/StemPendulum/StemPendulumToolbox.py index 4ced153..96a1a89 100644 --- a/sofagym/envs/StemPendulum/StemPendulumToolbox.py +++ b/sofagym/envs/StemPendulum/StemPendulumToolbox.py @@ -88,7 +88,7 @@ def getReward(self): r = -float(dist)/(2*self.beam_len) return r - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -152,17 +152,6 @@ def getState(rootNode): return state -class goalSetter(Sofa.Core.Controller): - def __init__(self, *args, **kwargs): - Sofa.Core.Controller.__init__(self, *args, **kwargs) - - def update(self): - pass - - def set_mo_pos(self, goal): - pass - - def getReward(rootNode): r = rootNode.Reward.getReward() return False, r diff --git a/sofagym/envs/Trunk/TrunkEnv.py b/sofagym/envs/Trunk/TrunkEnv.py index 5988b9a..810e587 100644 --- a/sofagym/envs/Trunk/TrunkEnv.py +++ b/sofagym/envs/Trunk/TrunkEnv.py @@ -12,14 +12,15 @@ import numpy as np import sys -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces +from typing import Optional -class TrunkEnv(AbstractEnv): +class TrunkEnv: """Sub-class of AbstractEnv, dedicated to the trunk scene. See the class AbstractEnv for arguments and methods. @@ -27,10 +28,12 @@ class TrunkEnv(AbstractEnv): # Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 66 DEFAULT_CONFIG = {"scene": "Trunk", "deterministic": True, "source": [300, 0, 80], "target": [0, 0, 80], + "goal": True, "goalList": [[40, 40, 100], [-10, 20, 80]], "start_node": None, "scale_factor": 5, @@ -45,51 +48,55 @@ class TrunkEnv(AbstractEnv): "discrete": True, "seed": None, "start_from_history": None, - "python_version": "python3", - "dt": 0.01 + "python_version": sys.version, + "dt": 0.01, + "nb_actions": 16, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config = None): - super().__init__(config) - nb_actions = 16 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - dim_state = 66 - low_coordinates = np.array([-1]*dim_state) - high_coordinates = np.array([1]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype='float32') + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() - def step(self, action): - return super().step(action) + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) - def reset(self): - """Reset simulation. - - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - - """ - super().reset() - - self.config.update({'goalPos': self.goal}) - obs = start_scene(self.config, self.nb_actions) + low_coordinates = np.array([-1]*self.env.dim_state) + high_coordinates = np.array([1]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - return np.array(obs['observation']) + if self.env.root is None and not self.use_server: + self.env.init_root() - def get_available_actions(self): - """Gives the actions available in the environment. + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) - Parameters: - ---------- - None. - - Returns: - ------- - list of the action available in the environment. + def reset(self): + """Reset simulation. """ - return list(range(int(self.nb_actions))) - - + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + + self.env.reset() + + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/Trunk/TrunkToolbox.py b/sofagym/envs/Trunk/TrunkToolbox.py index 38af0dc..ce09fd6 100644 --- a/sofagym/envs/Trunk/TrunkToolbox.py +++ b/sofagym/envs/Trunk/TrunkToolbox.py @@ -81,7 +81,7 @@ def getReward(self): return min(reward**(1/2), 1.0), current_dist - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -95,7 +95,7 @@ def update(self): None. """ - + self.goal_pos = goal trunkTips = self._computeTips() self.init_dist = np.linalg.norm(np.array(trunkTips)-np.array(self.goal_pos)) self.prev_dist = self.init_dist @@ -160,7 +160,7 @@ def __init__(self, *args, **kwargs): if kwargs["goalPos"]: self.goalPos = kwargs["goalPos"] - def update(self): + def update(self, goal): """Set the position of the goal. This function is used as an initialization function. @@ -174,6 +174,7 @@ def update(self): None. """ + self.goalPos = goal with self.goalMO.position.writeable() as position: position += self.goalPos diff --git a/sofagym/envs/TrunkCup/TrunkCupEnv.py b/sofagym/envs/TrunkCup/TrunkCupEnv.py index edd049e..bd3f05a 100644 --- a/sofagym/envs/TrunkCup/TrunkCupEnv.py +++ b/sofagym/envs/TrunkCup/TrunkCupEnv.py @@ -8,15 +8,17 @@ __copyright__ = "(c) 2020, Inria" __date__ = "Oct 7 2020" -import os +import os, sys import numpy as np -from sofagym.AbstractEnv import AbstractEnv +from sofagym.AbstractEnv import AbstractEnv, ServerEnv from sofagym.rpc_server import start_scene from gym import spaces -class TrunkCupEnv(AbstractEnv): +from typing import Optional + +class TrunkCupEnv: """Sub-class of AbstractEnv, dedicated to the trunk scene. See the class AbstractEnv for arguments and methods. @@ -24,10 +26,12 @@ class TrunkCupEnv(AbstractEnv): # Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} + dim_state = 66 DEFAULT_CONFIG = {"scene": "TrunkCup", "deterministic": True, "source": [500.0, 0, 100], "target": [0, 0, 100], + "goal": True, "goalList": [[40, 40, 100], [-10, 20, 80]], "start_node": None, "scale_factor": 5, @@ -42,53 +46,55 @@ class TrunkCupEnv(AbstractEnv): "discrete": True, "seed": None, "start_from_history": None, - "python_version": "python3.8", - "dt": 0.01 + "python_version": sys.version, + "dt": 0.01, + "nb_actions": 16, + "dim_state": dim_state, + "randomize_states": False, + "init_states": [0] * dim_state, + "use_server": False } - def __init__(self, config=None): - super().__init__(config) - nb_actions = 16 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) - - dim_state = 66 - low_coordinates = np.array([-1]*dim_state) - high_coordinates = np.array([1]*dim_state) - self.observation_space = spaces.Box(low_coordinates, high_coordinates, - dtype='float32') + def __init__(self, config = None, root=None, use_server: Optional[bool]=None): + if use_server is not None: + self.DEFAULT_CONFIG.update({'use_server': use_server}) + self.use_server = self.DEFAULT_CONFIG["use_server"] + self.env = ServerEnv(self.DEFAULT_CONFIG, config, root=root) if self.use_server else AbstractEnv(self.DEFAULT_CONFIG, config, root=root) - def step(self, action): - return super().step(action) + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() - def reset(self): - """Reset simulation. - - Note: - ---- - We launch a client to create the scene. The scene of the program is - client_Env.py. - - """ - super().reset() + self.env.action_space = spaces.Discrete(self.env.nb_actions) + self.nb_actions = str(self.env.nb_actions) - self.config.update({'goalPos': self.goal}) - obs = start_scene(self.config, self.nb_actions) + low_coordinates = np.array([-1]*self.env.dim_state) + high_coordinates = np.array([1]*self.env.dim_state) + self.env.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype=np.float32) - return np.array(obs['observation']) + if self.env.root is None and not self.use_server: + self.env.init_root() - def get_available_actions(self): - """Gives the actions available in the environment. + # called when an attribute is not found: + def __getattr__(self, name): + # assume it is implemented by self.instance + return self.env.__getattribute__(name) - Parameters: - ---------- - None. - - Returns: - ------- - list of the action available in the environment. + def reset(self): + """Reset simulation. """ - return list(range(int(self.nb_actions))) - - - + self.initialize_states() + + if self.env.config["goal"]: + self.init_goal() + + self.env.reset() + + if self.use_server: + obs = start_scene(self.env.config, self.nb_actions) + state = np.array(obs['observation'], dtype=np.float32) + else: + state = np.array(self.env._getState(self.env.root), dtype=np.float32) + + return state diff --git a/sofagym/envs/TrunkCup/TrunkCupToolbox.py b/sofagym/envs/TrunkCup/TrunkCupToolbox.py index 11f0fb4..27a730a 100644 --- a/sofagym/envs/TrunkCup/TrunkCupToolbox.py +++ b/sofagym/envs/TrunkCup/TrunkCupToolbox.py @@ -79,7 +79,7 @@ def getReward(self): return self.init_dist - current_dist, current_dist - def update(self): + def update(self, goal=None): """Update function. This function is used as an initialization function. @@ -93,7 +93,7 @@ def update(self): None. """ - + self.goal_pos = goal bary = self._computeCupBary() self.init_dist = np.sqrt(np.sum((bary-self.goal_pos)**2)) @@ -156,7 +156,7 @@ def __init__(self, *args, **kwargs): if kwargs["goalPos"]: self.goalPos = kwargs["goalPos"] - def update(self): + def update(self, goal): """Set the position of the goal. This function is used as an initialization function. @@ -170,6 +170,7 @@ def update(self): None. """ + self.goalPos = goal with self.goalMO.position.writeable() as position: position += self.goalPos diff --git a/sofagym/simulate.py b/sofagym/simulate.py index 4ac2dab..b06b52f 100644 --- a/sofagym/simulate.py +++ b/sofagym/simulate.py @@ -61,22 +61,20 @@ def init_simulation(config, _startCmd=None, mode="simu_and_visu"): config.update({'render': render}) print(">> ... Done.") - # Init Reward and GoalSetter - root.GoalSetter.update() - root.Reward.update() - - try: - root.StateInitializer.init_state() - except AttributeError as error: - print(error) + if config["randomize_states"]: + root.StateInitializer.init_state(config["init_states"]) if 'time_before_start' in config: print(">> Time before start:", config["time_before_start"], "steps. Initialization ...") for i in range(config["time_before_start"]): Sofa.Simulation.animate(root, config["dt"]) print(">> ... Done.") - # Update Reward and GoalSetter - root.GoalSetter.update() + + # Update Reward and GoalSetter + if config["goal"]: + root.GoalSetter.update(config['goalPos']) + root.Reward.update(config['goalPos']) + else: root.Reward.update() return root @@ -106,11 +104,13 @@ def step_simulation(root, config, action, _startCmd, _getPos, viewer=None): The positions of object(s) in the scene. """ - goal = config['goalPos'] + if config["goal"]: + goal = config['goalPos'] + root.GoalSetter.set_mo_pos(goal) + render = config['render'] surface_size = config['display_size'] - root.GoalSetter.set_mo_pos(goal) # Create the command from action _startCmd(root, action, config["dt"]*(config["scale_factor"]-1)) diff --git a/sofagym/viewer.py b/sofagym/viewer.py index 7241f68..19c5866 100644 --- a/sofagym/viewer.py +++ b/sofagym/viewer.py @@ -62,7 +62,7 @@ class Viewer: """ - def __init__(self, env, surface_size, zFar=100, save_path=None, create_video=None, fps=10): + def __init__(self, env, root, surface_size, zFar=100, save_path=None, create_video=None, fps=10): """ Classic initialization of a class in python. @@ -97,7 +97,11 @@ def __init__(self, env, surface_size, zFar=100, save_path=None, create_video=Non self.agent_display = None self.frame = 0 - self.root = init_simulation(self.env.config, mode = 'visu') + if self.env.config["use_server"]: + self.root = init_simulation(self.env.config, mode = 'visu') + else: + self.root = root + scene = self.env.config['scene'] self._setPos = importlib.import_module("sofagym.envs."+scene+"."+scene+"Toolbox").setPos @@ -132,15 +136,21 @@ def render(self, pos=None): # Recovering an image and handling error cases try: if pos is None: - pos = get_position(self.env.past_actions)['position'] + if self.env.config["use_server"]: + pos = get_position(self.env.past_actions)['position'] + else: + pos = self.env.pos + self.frame += 1 + if pos == []: image = np.zeros((self.surface_size[0], self.surface_size[1], 3)) else: num_im = 0 for p in pos: - self._setPos(self.root, p) - Sofa.Simulation.animate(self.root, self.root.getDt()) + if self.env.config["use_server"]: + self._setPos(self.root, p) + Sofa.Simulation.animate(self.root, self.root.getDt()) glViewport(0, 0, self.surface_size[0], self.surface_size[1])