From fc4bd9033c943d4422c1bc285a5d64cd9a48ffd3 Mon Sep 17 00:00:00 2001 From: hunoutl Date: Wed, 23 Nov 2022 14:45:55 +0100 Subject: [PATCH 1/4] [sofagym] First try on gymnasium --- README.md | 11 ++ sofagym/AbstractEnv.py | 38 ++++--- sofagym/envs/Gripper/GripperEnv.py | 9 +- sofagym/envs/Gripper/GripperScene.py | 17 +-- sofagym/envs/Gripper/GripperScene.py.log | 3 + sofagym/envs/Gripper/lastUsedGUI.ini | 1 + sofagym/envs/Gripper/runSofa.ini | 1 + sofagym/envs/Trunk/TrunkEnv.py | 7 +- sofagym/envs/Trunk/TrunkScene.py | 2 +- test_env.py | 128 ++++++++++++----------- 10 files changed, 128 insertions(+), 89 deletions(-) create mode 100644 sofagym/envs/Gripper/GripperScene.py.log create mode 100644 sofagym/envs/Gripper/lastUsedGUI.ini create mode 100644 sofagym/envs/Gripper/runSofa.ini diff --git a/README.md b/README.md index b50f3e2..6e5a7e6 100644 --- a/README.md +++ b/README.md @@ -138,6 +138,17 @@ It is possible to define new environments using SofaGym. For this purpose differ These different elements make it possible to create and personalise the task to be performed. See examples of environments for implementation. ## The environments +``` +dev status: +* multigaitrobot-v0 [ERROR] [RequiredPlugin(ModelOrderReduction)] Plugin not found: "ModelOrderReduction" +* gripper-v0 [ERROR] [TetrahedronSetTopologyContainer(container)] Cannot find edge +* trunk-v0 #error +* trunkcup-v0 #[ERROR] [TetrahedronSetTopologyContainer(container)] Cannot find edge +* diamondrobot-v0 #WORKING, only deprecated feature left +* maze-v0 #[ERROR] [SofaRuntime] NotImplementedError: Importing your SOFA Scene Failed +* simple_maze-v0 #[ERROR] [SofaRuntime] ValueError: Object type Sphere<> was not created +* concentrictuberobot-v0 #WORKING, only deprecated feature left. Can crash with solver error=-nan +``` ### Gripper diff --git a/sofagym/AbstractEnv.py b/sofagym/AbstractEnv.py index ef737de..531f57d 100644 --- a/sofagym/AbstractEnv.py +++ b/sofagym/AbstractEnv.py @@ -8,8 +8,8 @@ __copyright__ = "(c) 2020, Robocath, CNRS, Inria" __date__ = "Oct 7 2020" -import gym -from gym.utils import seeding +import gymnasium as gym +from gymnasium.utils import seeding import numpy as np import copy @@ -147,7 +147,7 @@ def initialization(self): self.goal = None self.past_actions = [] - + self.num_envs = 40 self.np_random = None @@ -156,6 +156,7 @@ def initialization(self): self.viewer = None self.automatic_rendering_callback = None + self.timer = 0 self.timeout = self.config["timeout"] @@ -281,13 +282,20 @@ def step(self, action): Returns: ------- - obs: + obs(ObsType): The new state of the agent. - reward: + reward(float): The reward obtain after applying the action in the current state. - done: + terminated(bool): + Whether the agent reaches the terminal state + truncated(bool): + Whether the truncation condition outside the scope of the MDP is satisfied. + Typically, this is a timelimit, but could also be used to indicate an agent physically going out of bounds. + info(dict): + additional information (not used here) + done(bool)(Deprecated): Whether the goal is reached or not. - {}: additional information (not used here) + """ # assert self.action_space.contains(action), "%r (%s) invalid" % (action, type(action)) @@ -301,20 +309,22 @@ def step(self, 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"] + terminated = results["done"] # Avoid long explorations by using a timer. + truncated = False self.timer += 1 if self.timer >= self.config["timer_limit"]: # reward = -150 - done = True + truncated = True + info={}#(not use here) if self.config["planning"]: self.clean() - - return obs, reward, done, {} + return obs, reward, terminated, truncated, info def async_step(self, action): """Executes one action in the environment. @@ -363,7 +373,7 @@ def reset(self): Returns: ------- - None. + obs, info """ self.close() @@ -380,8 +390,8 @@ def reset(self): self.timer = 0 self.past_actions = [] - - return + + return def render(self, mode='rgb_array'): """See the current state of the environment. diff --git a/sofagym/envs/Gripper/GripperEnv.py b/sofagym/envs/Gripper/GripperEnv.py index 8e2da8a..688a343 100644 --- a/sofagym/envs/Gripper/GripperEnv.py +++ b/sofagym/envs/Gripper/GripperEnv.py @@ -11,9 +11,9 @@ from sofagym.AbstractEnv import AbstractEnv from sofagym.rpc_server import start_scene -from gym.envs.registration import register +from gymnasium.envs.registration import register -from gym import spaces +from gymnasium import spaces import os import numpy as np @@ -58,6 +58,8 @@ def __init__(self, config=None): high_coordinates = np.array([1]*dim_state) self.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype='float32') + + def step(self, action): return super().step(action) @@ -75,8 +77,9 @@ def reset(self): self.config.update({'goalPos': self.goal}) obs = start_scene(self.config, self.nb_actions) + info = {} - return obs['observation'] + return (obs['observation'], info) def get_available_actions(self): """Gives the actions available in the environment. diff --git a/sofagym/envs/Gripper/GripperScene.py b/sofagym/envs/Gripper/GripperScene.py index 6c2a554..a019a0c 100644 --- a/sofagym/envs/Gripper/GripperScene.py +++ b/sofagym/envs/Gripper/GripperScene.py @@ -197,16 +197,17 @@ def main(): SofaRuntime.importPlugin("SofaImplicitOdeSolver") - for i in range (6000): - root=Sofa.Core.Node("root") - createScene(root) - Sofa.Simulation.init(root) + #for i in range (6000): + # print("Simulation N°",i) + root=Sofa.Core.Node("root") + createScene(root) + Sofa.Simulation.init(root) - # Run the simulation for 10 steps - for iteration in range(10): - Sofa.Simulation.animate(root, root.dt.value) + # Run the simulation for 10 steps + #for iteration in range(10): + # Sofa.Simulation.animate(root, root.dt.value) - print("Simulation N°",i) + Sofa.Simulation.init(root) diff --git a/sofagym/envs/Gripper/GripperScene.py.log b/sofagym/envs/Gripper/GripperScene.py.log new file mode 100644 index 0000000..d5a6a94 --- /dev/null +++ b/sofagym/envs/Gripper/GripperScene.py.log @@ -0,0 +1,3 @@ + +--- NEW SESSION: 23/11/2022 13:59:27 --- +/Goal/VisualStyle.displayFlags = showVisual hideBehavior showCollisionModels hideBoundingCollisionModels hideMapping hideOptions diff --git a/sofagym/envs/Gripper/lastUsedGUI.ini b/sofagym/envs/Gripper/lastUsedGUI.ini new file mode 100644 index 0000000..215671c --- /dev/null +++ b/sofagym/envs/Gripper/lastUsedGUI.ini @@ -0,0 +1 @@ +qglviewer diff --git a/sofagym/envs/Gripper/runSofa.ini b/sofagym/envs/Gripper/runSofa.ini new file mode 100644 index 0000000..8380bec --- /dev/null +++ b/sofagym/envs/Gripper/runSofa.ini @@ -0,0 +1 @@ +/home/hunoutl/SOFA/SofaGym/sofagym/envs/Gripper/GripperScene.py diff --git a/sofagym/envs/Trunk/TrunkEnv.py b/sofagym/envs/Trunk/TrunkEnv.py index 72ad506..b34fc9b 100644 --- a/sofagym/envs/Trunk/TrunkEnv.py +++ b/sofagym/envs/Trunk/TrunkEnv.py @@ -11,9 +11,9 @@ from sofagym.AbstractEnv import AbstractEnv from sofagym.rpc_server import start_scene -from gym.envs.registration import register +from gymnasium.envs.registration import register -from gym import spaces +from gymnasium import spaces import os import sys import numpy as np @@ -57,8 +57,7 @@ def __init__(self, config = None): 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.observation_space = spaces.Box(low_coordinates, high_coordinates, dtype='float32') def step(self, action): return super().step(action) diff --git a/sofagym/envs/Trunk/TrunkScene.py b/sofagym/envs/Trunk/TrunkScene.py index 5fa9b11..56d0c6c 100644 --- a/sofagym/envs/Trunk/TrunkScene.py +++ b/sofagym/envs/Trunk/TrunkScene.py @@ -195,7 +195,7 @@ def createScene(rootNode, config={"source": [-600.0, -25, 100], response='FrictionContactConstraint') rootNode.addObject('LocalMinDistance', alarmDistance=10, contactDistance=5, angleCone=0.01) - rootNode.addObject(AnimationManagerController(name="AnimationManager")) + rootNode.addObject(AnimationManagerController(rootNode)) rootNode.gravity.value = [0., -9810., 0.] diff --git a/test_env.py b/test_env.py index f3eb44f..39bb040 100644 --- a/test_env.py +++ b/test_env.py @@ -15,7 +15,7 @@ import sys import os import time -import gym +import gymnasium as gym from sofagym import * from sofagym.envs import * @@ -28,15 +28,16 @@ sys.path.insert(0, os.getcwd()+"/..") __import__('sofagym') -name = ['multigaitrobot-v0',#[ERROR] [RequiredPlugin(ModelOrderReduction)] Plugin not found: "ModelOrderReduction" - 'gripper-v0', #[ERROR] [TetrahedronSetTopologyContainer(container)] Cannot find edge - 'trunk-v0', #error - 'trunkcup-v0', #[ERROR] [TetrahedronSetTopologyContainer(container)] Cannot find edge - 'diamondrobot-v0', # WORKING, only deprecated feature left - 'maze-v0', #[ERROR] [SofaRuntime] NotImplementedError: Importing your SOFA Scene Failed - 'simple_maze-v0', #[ERROR] [SofaRuntime] ValueError: Object type Sphere<> was not created - 'concentrictuberobot-v0'] # WORKING, only deprecated feature left. Can crash with solver error=-nan -num = 1 +name = {0:'multigaitrobot-v0', + 1:'gripper-v0', + 2:'trunk-v0', + 3:'trunkcup-v0', + 4:'diamondrobot-v0', + 5:'maze-v0', + 6:'simple_maze-v0', + 7:'concentrictuberobot-v0'} + +num = 2 env_name = name[num] print("Start env ", env_name) @@ -48,6 +49,63 @@ env.render() done = False +print("Start ...") +for i in range(10000000): + print("\n--------------------------------") + print("EPISODE - ", i) + print("--------------------------------\n") + idx = 0 + tot_reward = 0 + tot_rtf = 0 + done = False + while not done and idx < 100: + idx += 1 + #multigaitrobot: [rd.uniform(-1, 1) for i in range(5)] - strat_multi[idx-1] + #gripper: rd.randint(0,7) + #trunk: rd.randint(0,15) + #trunkcup: rd.randint(0,15) + #diamondrobot: rd.randint(0,7) + #maze: rd.randint(0,6) + #simple_maze: rd.randint(0,3) + #concentrictuberobot: rd.randint(0,11) + #CartStem [rd.uniform(-1, 1)] + #action = strat_multi[idx-1] + + action = env.action_space.sample() + + start_time = time.time() + state, reward, terminated, _, _ = env.step(action) + step_time = time.time()-start_time + print("[INFO] >>> Time:", step_time) + + rtf = env.config["dt"]*env.config["scale_factor"]/step_time + print("[INFO] >>> RTF:", rtf) + + tot_reward+= reward + tot_rtf+= rtf + + env.render() + + print("Step ", idx, " action : ", action, " reward : ", reward, " done:", terminated) + + print("[INFO] >>> TOTAL REWARD IS:", tot_reward) + print("[INFO] >>> FINAL REWARD IS:", reward) + print("[INFO] >>> MEAN RTF IS:", tot_rtf/idx) + memoryUse = py.memory_info()[0]/2.**30 + print("[INFO] >>> Memory usage:", memoryUse) + print("[INFO] >>> Object size:", sys.getsizeof(env)) + + env.reset() + + +print(">> TOTAL REWARD IS:", tot_reward) +env.close() +print("... End.") + + + + +''' strat_multi = [[-1.0, -1.0, -1.0, 1, 1], [1.0, -1.0, -1.0, 1, 1], [1.0, 1.0, 1.0, 1, 1], [1.0, 1.0, 1.0, -1.0, -1.0], [-1.0, 1.0, 1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0, -1.0], @@ -105,52 +163,4 @@ [-1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0], [-0.8, 0.2, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0]] + [[-0.8, 0.2, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0]]*100 - -print("Start ...") -for i in range(10000000): - print("\n--------------------------------") - print("EPISODE - ", i) - print("--------------------------------\n") - idx = 0 - tot_reward = 0 - tot_rtf = 0 - done = False - while not done and idx < 100: - idx += 1 - #multigaitrobot: [rd.uniform(-1, 1) for i in range(5)] - strat_multi[idx-1] - #gripper: rd.randint(0,7) - #trunk: rd.randint(0,15) - #trunkcup: rd.randint(0,15) - #diamondrobot: rd.randint(0,7) - #maze: rd.randint(0,6) - #simple_maze: rd.randint(0,3) - #concentrictuberobot: rd.randint(0,11) - #CartStem [rd.uniform(-1, 1)] - - action = strat_multi[idx-1] - action = env.action_space.sample() - start_time = time.time() - state, reward, done, _ = env.step(action) - step_time = time.time()-start_time - print("[INFO] >>> Time:", step_time) - rtf = env.config["dt"]*env.config["scale_factor"]/step_time - print("[INFO] >>> RTF:", rtf) - tot_reward+= reward - tot_rtf+= rtf - env.render() - - print("Step ", idx, " action : ", action, " reward : ", reward, " done:", done) - - print("[INFO] >>> TOTAL REWARD IS:", tot_reward) - print("[INFO] >>> FINAL REWARD IS:", reward) - print("[INFO] >>> MEAN RTF IS:", tot_rtf/idx) - memoryUse = py.memory_info()[0]/2.**30 - print("[INFO] >>> Memory usage:", memoryUse) - print("[INFO] >>> Object size:", sys.getsizeof(env)) - - env.reset() - - -print(">> TOTAL REWARD IS:", tot_reward) -env.close() -print("... End.") +''' \ No newline at end of file From 307579e0acfb6cf3a636f5f2a0ed7354e89b268a Mon Sep 17 00:00:00 2001 From: hunoutl Date: Fri, 6 Jan 2023 17:00:23 +0100 Subject: [PATCH 2/4] [all] readme, envs & import --- README.md | 152 +++++++----------- plot.py => Tools/plot.py | 0 dist/sofagym-0.0.1-py3-none-any.whl | Bin 0 -> 1290 bytes requierements.txt | 2 - sofagym/__init__ .py | 2 +- .../{BaseTemplate.py => BaseTemplateEnv.py} | 17 +- .../envs/BaseTemplate/BaseTemplateScene.py | 114 ------------- sofagym/envs/BaseTemplate/__init__.py | 0 sofagym/envs/BubbleMotion/BubbleMotionEnv.py | 9 +- sofagym/envs/CTR/CTREnv.py | 8 +- sofagym/envs/CartStem/CartStem.py | 4 +- sofagym/envs/CartStem/CartStemEnv.py | 8 +- .../envs/CartStemContact/CartStemContact.py | 7 +- .../CartStemContact/CartStemContactEnv.py | 11 +- sofagym/envs/CatchTheObject/CatchTheObject.py | 5 +- .../envs/CatchTheObject/CatchTheObjectEnv.py | 12 +- .../CatchTheObject/CatchTheObjectScene.py | 3 +- sofagym/envs/Diamond/DiamondEnv.py | 12 +- sofagym/envs/Gripper/GripperEnv.py | 4 +- sofagym/envs/Maze/MazeEnv.py | 14 +- .../envs/MultiGaitRobot/MultiGaitRobotEnv.py | 12 +- sofagym/envs/SimpleMaze/SimpleMazeEnv.py | 10 +- sofagym/envs/StemPendulum/StemPendulum.py | 4 +- sofagym/envs/StemPendulum/StemPendulumEnv.py | 12 +- .../envs/StemPendulum/StemPendulumToolbox.py | 2 +- sofagym/envs/Trunk/TrunkEnv.py | 13 +- sofagym/envs/TrunkCup/TrunkCupEnv.py | 12 +- sofagym/envs/__init__.py | 77 ++++++++- test_env.py | 134 ++------------- 29 files changed, 199 insertions(+), 461 deletions(-) rename plot.py => Tools/plot.py (100%) create mode 100644 dist/sofagym-0.0.1-py3-none-any.whl rename sofagym/envs/BaseTemplate/{BaseTemplate.py => BaseTemplateEnv.py} (92%) delete mode 100644 sofagym/envs/BaseTemplate/__init__.py diff --git a/README.md b/README.md index 153b2af..80b1dfc 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ # SofaGym -Software toolkit to easily create an [OpenAI Gym](https://github.com/openai/gym) environment out of any [SOFA](https://github.com/sofa-framework/sofa) scene. +Software toolkit to easily create an [Gymnasium](https://github.com/Farama-Foundation/Gymnasium) (previously Gym) environment out of any [SOFA](https://github.com/sofa-framework/sofa) scene. -The toolkit provides an API based on the standard OpenAI Gym API, allowing to train classical Reinforcement Learning algorithms. +The toolkit provides an API based on the standard Gymnasium API, allowing to train classical Reinforcement Learning algorithms. The toolkit also comprises example scenes based on the SoftRobots plugin for SOFA to illustrate how to include SOFA simulations and train learning algorithms on them. @@ -19,8 +19,7 @@ with some mandatory plugins : and optional plugins (for some examples): * [ModelOrderReduction](https://github.com/SofaDefrost/ModelOrderReduction) - -[comment]: <> (SoftRobots.Inverse and Cosserat) +* [Cosserat](https://github.com/SofaDefrost/plugin.Cosserat) [Plugins installation](https://www.sofa-framework.org/community/doc/plugins/build-a-plugin-from-sources/#in-tree-build) with a in-tree build is preferred. @@ -30,23 +29,22 @@ and optional plugins (for some examples): We use python3. mandatory : ```bash -pip install gym psutil pygame glfw pyopengl imageio +pip install gymnasium psutil pygame glfw pyopengl imageio ``` -* requierements.txt * [stable_baseline](https://github.com/DLR-RM/stable-baselines3) + optional : * [Actor with Variance Estimated Critic (AVEC)](https://github.com/yfletberliac/actor-with-variance-estimated-critic) -### Install +### Install Sofagym ```bash python setup.py bdist_wheel pip install -v -e . ``` - ## Quick start ```bash @@ -57,31 +55,30 @@ export PYTHONPATH=/sofa/build_dir/lib/python3/site-packages:$PYTHONPATH import sofagym ``` -## Usage - The Gym framework allows to interact with an environment using well-known keywords: - *step(a)*: allows to perform a simulation step when the agent performs the action *a*. Given the current state of the system *obs_t* and the action *a*, the environment then changes to a new state *obs_{t+1}* and the agent receives the reward *rew*. If the goal is reached, the *done* flag changes to *True*. - *reset*: resets the environment. - *render*: gives a visual representation of *obs_t*. -The use of this interface allows intuitive interaction with any environment, and this is what SofaGym allows when the environment is a Sofa scene. For more information on Gym, check the official documentation page [here](https://gym.openai.com/docs/). +The use of this interface allows intuitive interaction with any environment, and this is what SofaGym allows when the environment is a Sofa scene. For more information on Gymnasium, check the official [documentation page](https://gymnasium.farama.org/). -Example of use +Example of use : ```python -import gym +import gymnasium as gym import sofagym.envs env = gym.make('trunk-v0') -env.reset() +observation, info = env.reset(seed=42) done = False while not done: - action = ... # Your agent code here - state, reward, done, info = env.step(action) + action = env.action_space.sample() # this is where you would insert your policy + observation, reward, terminated, truncated, info = env.step(action) env.render() - print("Step ", idx, " done : ",done, " state : ", state, " reward : ", reward) + done = terminated or truncated + env.close() ``` @@ -93,43 +90,32 @@ The classic running of an episode is therefore: -## Citing - -If you use the project in your work, please consider citing it with: -```bibtex -@misc{SofaGym, - authors = {Ménager, Etienne and Schegg, Pierre and Duriez, Christian and Marchal, Damien}, - title = {SofaGym: An OpenAI Gym API for SOFASimulations}, - year = {2020}, - publisher = {GitHub}, - journal = {GitHub repository}, -} -``` -## The tools - -### Server/worker architecture - -The major difficulty encountered in this work is the fact that it is not possible to copy the *root* from a Sofa simulation. This implies that when two sequences of actions *A_1 = [a_1, ..., a_n, new_action_1]* and *A_2 = [a_1, ..., a_n, new_action_2]* have to be tried, it is necessary to start again from the beginning each time and simulate again *[a_1, ..., a_n]*. This leads to a huge loss of performance. To solve this problem a server/worker architecture is set up. - -A server takes care of distributing the calculations between several clients. Each client *i* is associated with an action sequence *A_i = [a_{i1}, ...., a_{in}]*. Given an action sequence *A = [a_{1}, ...., a_{n}]* and a new action *a*, the server looks for the client with the action sequence *A_i*. This client forks and the child executes the new action *a*. The father and son are referenced to the server as two separate clients and the action sequence *[a_{1}, ...., a_{n}]* and *[a_{1}, ...., a_{n}, a]* can be accessed. - -A cleaning system is used to close clients that are no longer used. This makes it possible to avoid having an exponential number of open clients. - -When it is not necessary to have access to the different states of the environment, i.e. when the actions are used sequentially, only one client is open and performs the calculations sequentially. +## The environments -### Vectorized environment +|Image|Name|Description|Status| +|----------|:-------------|:-------------|:-------------| +| |[BaseTemplate](sofagym/envs/BaseTemplate/) basetemplate-v0| A base template|NOT DONE| +| |[BubbleMotion](sofagym/envs/BubbleMotion/) bubblemotion-v0| |Plugin not found: "CosseratPlugin"| +| |[CartStem](sofagym/envs/CartStem/) cartstem-v0| |Plugin not found: "CosseratPlugin"| +| |[CartStemContact](sofagym/envs/CartStemContact/) cartstemcontact-v0| | Plugin not found: "CosseratPlugin"| +| |[CatchTheObject](sofagym/envs/CatchTheObject/) catchtheobject-v0| |Plugin not found: "CosseratPlugin"| +| |[ConcentricTubeRobot](sofagym/envs/CTR/) concentrictuberobot-v0| |OK | +| |[DiamondRobot](sofagym/envs/Diamond/) diamondrobot-v0| |OSError: [Errno 36] File name too long| +| |[Gripper](sofagym/envs/Gripper/) gripper-v0| The objective is to grasp a cube and bring it to a certain height. The closer the cube is to the target, the greater the reward.| OK| +| |[Maze](sofagym/envs/Maze/) maze-v0| The Maze environment offers one scene of a ball navigating in a maze. The maze is attached to the tripod robot and the ball is moved by gravity by modifying the maze’s orientation. The tripod is actuated by three servomotors. Similarly to the Trunk Environment, the Maze environment has a dicrete action space of 6 actions, moving each servomotor by one increment, and could easily be extended to be continuous.|Importing your SOFA Scene Failed| +| |[MultiGait Robot](sofagym/envs/MultiGaitRobot/) multigaitrobot-v0| The multigait Softrobot has one scene. The goal is to move the robot forward in the *x* direction with the highest speed. env = gym.make("multigaitrobot-v0")|ERROR| +| |[SimpleMaze](sofagym/envs/SimpleMaze/) simple_maze-v0| |ValueError: Object type Sphere<> was not created | +| |[StemPendulum](sofagym/envs/StemPendulum/) stempendulum-v0| |NotImplementedError: Importing your SOFA Scene Failed | +| |[Trunk](sofagym/envs/Trunk/) trunk-v0| The Trunk environment offers two scenarios. Both are based on the trunk robot. The first is to bring the trunk’s tip to a certain position. The second scenario is to manipulate a cup using the trunk to get the cup’s center of gravity in a predefined position. The Trunk is controlled by eight cables that can be contracted or extended by one unit. There are therefore 16 possible actions. The action space presented here is discrete but could easily be ex-tended to become continuous.|ERROR | +| |[TrunkCup](sofagym/envs/TrunkCup/) trunkcup-v0| |OK| -Simulation training can be time consuming. It is therefore necessary to be able to parallelise the calculations. Since the actions are chosen sequentially, it is not possible to parallelise the calculations for one environment. The result depends on the previous result. However, it is possible to parallelise on several environments, meaning to run several simulations in parallel. This is done with the baseline of OpenAI: SubprocVecEnv. -### Separation between visualisation and computations -SofaGym separates calculations and visualisation. In order to achieve this, two scenes must be created: a scene *A* with all visual elements and a scene *B* with calculation elements (solvers, ...). Scene *A* is used in a viewer and scene *B* in the clients. Once the calculations have been performed in scene *B*, the positions of the points are given to the viewer which updates scene *A*. ### Adding new environment - It is possible to define new environments using SofaGym. For this purpose different elements have to be created: - *NameEnv*: inherits from *AbstractEnv*. It allows to give the specificity of the environment like the action domain (discrete or continuous) and the configuration elements. - *NameScene*: allows to create the Sofa scene. It must have the classic createScene function and return a *root*. To improve performance it is possible to separate the visual and computational aspects of the scene using the *mode* parameter (*'visu'* or *'simu'*). It allows you to choose the elements in the viewer-related scene or in the client-related scene. We also integrate two Sofa.Core.Controller (rewardShaper and goalSetter) that allow to integrate goal and reward in the scene. @@ -137,70 +123,28 @@ It is possible to define new environments using SofaGym. For this purpose differ These different elements make it possible to create and personalise the task to be performed. See examples of environments for implementation. -## The environments -``` -dev status: -* multigaitrobot-v0 [ERROR] [RequiredPlugin(ModelOrderReduction)] Plugin not found: "ModelOrderReduction" -* gripper-v0 [ERROR] [TetrahedronSetTopologyContainer(container)] Cannot find edge -* trunk-v0 #error -* trunkcup-v0 #[ERROR] [TetrahedronSetTopologyContainer(container)] Cannot find edge -* diamondrobot-v0 #WORKING, only deprecated feature left -* maze-v0 #[ERROR] [SofaRuntime] NotImplementedError: Importing your SOFA Scene Failed -* simple_maze-v0 #[ERROR] [SofaRuntime] ValueError: Object type Sphere<> was not created -* concentrictuberobot-v0 #WORKING, only deprecated feature left. Can crash with solver error=-nan -``` - -### Gripper - -The Gripper Environmentoffers two different scenes. In both scenes, the objective is to grasp a cube and bring it to a certain height. The closer the cube is to the target, the greater the reward. - -The two scenes are distinguished by their action space. In one case the actions are discrete and correspond to a particular movement. We define a correspondence between a Gym action (int) and corresponding Sofa displacement and direction. -```python -env = gym.make("gripper-v0") -``` - -In the second case, the actions are continuous and correspond directly to a movement ofthe gripper’s fingers. This difference is indicated when defining the environment - -```python -env = gym.make("continuegripper-v0") -``` - -### Trunk - -The Trunk environment offers two scenarios. Both are based on the trunk robot. The first is to bring the trunk’s tip to a certain position. - -```python -env = gym.make("trunk-v0") -``` - -The second scenario is to manipulate a cup using the trunk to get the cup’s center of gravity in a predefined position. +## The tools -```python -env = gym.make("trunkcup-v0") -``` +### Server/worker architecture -The Trunk is controlled by eight cables that can be contracted or extended by one unit. There are therefore 16 possible actions. The action space presented here is discrete but could easily be ex-tended to become continuous. +The major difficulty encountered in this work is the fact that it is not possible to copy the *root* from a Sofa simulation. This implies that when two sequences of actions *A_1 = [a_1, ..., a_n, new_action_1]* and *A_2 = [a_1, ..., a_n, new_action_2]* have to be tried, it is necessary to start again from the beginning each time and simulate again *[a_1, ..., a_n]*. This leads to a huge loss of performance. To solve this problem a server/worker architecture is set up. +A server takes care of distributing the calculations between several clients. Each client *i* is associated with an action sequence *A_i = [a_{i1}, ...., a_{in}]*. Given an action sequence *A = [a_{1}, ...., a_{n}]* and a new action *a*, the server looks for the client with the action sequence *A_i*. This client forks and the child executes the new action *a*. The father and son are referenced to the server as two separate clients and the action sequence *[a_{1}, ...., a_{n}]* and *[a_{1}, ...., a_{n}, a]* can be accessed. -### MultiGait Robot +A cleaning system is used to close clients that are no longer used. This makes it possible to avoid having an exponential number of open clients. -The multigait Softrobot has one scene. The goal is to move the robot forward in the *x* direction with the highest speed. +When it is not necessary to have access to the different states of the environment, i.e. when the actions are used sequentially, only one client is open and performs the calculations sequentially. -```python -env = gym.make("multigaitrobot-v0") -``` +### Vectorized environment -### Maze -The Maze environment offers one scene of a ball navigating in a maze. The maze is attached to the tripod robot and the ball is moved by gravity by modifying the maze’s orientation. +Simulation training can be time consuming. It is therefore necessary to be able to parallelise the calculations. Since the actions are chosen sequentially, it is not possible to parallelise the calculations for one environment. The result depends on the previous result. However, it is possible to parallelise on several environments, meaning to run several simulations in parallel. This is done with the baseline of OpenAI: SubprocVecEnv. -```python -env = gym.make("maze-v0") -``` -The tripod is actuated by three servomotors. Similarly to the Trunk Environment, the Maze environment has a dicrete action space of 6 actions, moving each servomotor by one increment, and could easily be extended to be continuous. +### Separation between visualisation and computations +SofaGym separates calculations and visualisation. In order to achieve this, two scenes must be created: a scene *A* with all visual elements and a scene *B* with calculation elements (solvers, ...). Scene *A* is used in a viewer and scene *B* in the clients. Once the calculations have been performed in scene *B*, the positions of the points are given to the viewer which updates scene *A*. ## Results @@ -211,6 +155,20 @@ In this section we demonstrate some use cases of the environments available in S ### Monte Carlo Tree Search: solving MazeEnv with planning +## Citing + +If you use the project in your work, please consider citing it with: +```bibtex +@misc{SofaGym, + authors = {Ménager, Etienne and Schegg, Pierre and Duriez, Christian and Marchal, Damien}, + title = {SofaGym: An OpenAI Gym API for SOFASimulations}, + year = {2020}, + publisher = {GitHub}, + journal = {GitHub repository}, +} +``` + + ## Notes 1. At the moment the available action spaces are: continuous, discrete, tuple and dictionary. diff --git a/plot.py b/Tools/plot.py similarity index 100% rename from plot.py rename to Tools/plot.py diff --git a/dist/sofagym-0.0.1-py3-none-any.whl b/dist/sofagym-0.0.1-py3-none-any.whl new file mode 100644 index 0000000000000000000000000000000000000000..7da1a48e8375e8b91ce7838a9effc8708e400655 GIT binary patch literal 1290 zcmWIWW@Zs#U|`^2_+F|K_N`Qbc?XcE!N|a%0Hlla(-PAwb9D{$4D<~3QZkE6bTjkP z^7VaPLmXWkLmX2l9Lzdwz|;1=j%%-j)3)TiMnkr{mo9C1w??(g(4{ELn!E0I=b>w6 zwy%CK_4kmUQgEQO)+SV)V>@}M_}(GQ=-NiZ`mE2~0VuwH znIY8>1N1Ej%i{5MxQDB&5BGTw-?JBay>+$DojJcb$l!|cgChU4-nw2oC-pZ4d3c@F z(d*P_Um1A8;DWK)b>k~fxK5qdzxY+>8n1?~*V&Ukn}RenwS1p)dHQS*(pX_~?d<9E z-dBCktbX!{i2>DF=d!l%5Cb{~6g?VvoRykaQdAjVke``XQmj`}QPS;q?!5N7^Pau> zetM^LG&K93KEg12GTU_{W}unCcxF(=V|Gb?L3~bXS!xc*;K%4L{8HE@{2Qpd9cYXk z9#eu`o&AGcO3ye>cXD80cu>7RH6rBEhIUV>+%?m`=EUsDa9Mot49DMJwqNhX)!09H z>|40gm)~=uXynxOqT)fW-OmnhcHYsf$g*coM)qH0L(TZUjI(^K?}J)SOIa5NKYJ*` zYp3+aAT+7`yU}0IhO}4QcAx)QZF{>fc*(1CTzdRFENxeuf0S|cF^g>dyVR@m+#gSs z-eUfJMO~G^j9YIyS0)7CG7}Yz5nEjGeTTD2NsDad$ACG?i`15dN42f5m3F>=ut&4> z=v{VYW%gM{x4W15X8$?z^}lr5+U8HQXM3tVi0|O5p3Sl8Wqj_V=$~fy)-0*0PCd?1 zxZvEi#@wF=`5KPO2Y53wi7?~NXTY!ngDs69icqdYHw``gLd;@d*wXk7k7?l4jBXTq znnM`X3yfo24nt3Y=%%8lHiW4!@tBH|_Rx()PkL}88TFZPI~0--1H4(;K> TOTAL REWARD IS:", tot_reward) env.close() print("... End.") - - - - -''' -strat_multi = [[-1.0, -1.0, -1.0, 1, 1], [1.0, -1.0, -1.0, 1, 1], - [1.0, 1.0, 1.0, 1, 1], [1.0, 1.0, 1.0, -1.0, -1.0], - [-1.0, 1.0, 1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0, -1.0], - [-1.0, -1.0, -1.0, 1, 1], [1.0, -1.0, -1.0, 1, 1], - [1.0, 1.0, 1.0, 1, 1], [1.0, 1.0, 1.0, -1.0, -1.0], - [-1.0, 1.0, 1.0, -1.0, -1.0], [-1.0, -1.0, -1.0,-1.0, -1.0]]*100 -# strat_multi = [[1.0, 1.0, 1.0, 1, 1]]*100 - - -# strat_multi = [[-1.0, -1.0, -1.0, 1, 1], [1.0, -1.0, -1.0, 1, 1], -# [1.0, 1.0, 1.0, 1, 1], [1.0, 1.0, 1.0, -1.0, -1.0], -# [-1.0, 1.0, 1.0, -1.0, -1.0], [-1.0, -1.0, -1.0, -1.0, -1.0], -# [-1.0, 1, -1.0, -1.0, 1], [-1.0, -1, 1.0, 1.0, -1], -# [1.0, 1, -1.0, -1.0, 1], [-1.0, -1.0, 1.0, -1.0, 1], -# [1.0, -1.0, 1.0, -1.0, -1.0], [-1.0, -1, 1.0, 1.0, -1], -# [1.0, 1.0, -1.0, -1.0, -1.0], [-1.0, 1.0, -1.0, 1.0, -1], -# [1.0, -1, 1.0, 1.0, -1]] - -start_multi_discrete = [2, 0, 1, 5, 3, 4, - 2, 0, 1, 5, 3, 4, - 2, 0, 1, 5, 3, 4, - 2, 0, 1, 5, 3, 4, - 2, 0, 1, 5, 3, 4,] - -strat_jimmy_1 = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, -0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [-0.5, -0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [-0.6, -0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [-0.6, -0.5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0], - [0.0, 0.75, 0.75, 0.0, 0.0, 0.0, 1.0, 0.0]]+[[0.0, 0.75, 0.75, 0.0, 0.0, 0.0, 1.0, 0.0]]*100 - -strat_jimmy_0 = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0], - [0.0, 0.0, -1.0, 0.0, 0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0], - [0.0, 0.0, -1.0, 0.0, 0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [0.0, 0.0, -1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 1.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [0.0, 0.0, -1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 1.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [0.0, 0.0, -1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 1.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [0.0, 0.0, -1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 1.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [-1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0], - [-0.8, 0.2, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0]] + [[-0.8, 0.2, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0]]*100 - -''' -print("Start ...") -for i in range(10000000): - print("\n--------------------------------") - print("EPISODE - ", i) - print("--------------------------------\n") - idx = 0 - tot_reward = 0 - tot_rtf = 0 - done = False - while not done and idx < 100: - idx += 1 - #multigaitrobot: [rd.uniform(-1, 1) for i in range(5)] - strat_multi[idx-1] - #gripper: rd.randint(0,7) - #trunk: rd.randint(0,15) - #trunkcup: rd.randint(0,15) - #diamondrobot: rd.randint(0,7) - #maze: rd.randint(0,6) - #simple_maze: rd.randint(0,3) - #concentrictuberobot: rd.randint(0,11) - #CartStem [rd.uniform(-1, 1)] - #action = strat_multi[idx-1] - - action = env.action_space.sample() - start_time = time.time() - state, reward, done, _ = env.step(action) - step_time = time.time()-start_time - print("[INFO] >>> Time:", step_time) - rtf = env.config["dt"]*env.config["scale_factor"]/step_time - print("[INFO] >>> RTF:", rtf) - tot_reward+= reward - tot_rtf+= rtf - env.render() - - print("Step ", idx, " action : ", action, " reward : ", reward, " done:", done) - - print("[INFO] >>> TOTAL REWARD IS:", tot_reward) - print("[INFO] >>> FINAL REWARD IS:", reward) - print("[INFO] >>> MEAN RTF IS:", tot_rtf/idx) - memoryUse = py.memory_info()[0]/2.**30 - print("[INFO] >>> Memory usage:", memoryUse) - print("[INFO] >>> Object size:", sys.getsizeof(env)) - - env.reset() - - -print(">> TOTAL REWARD IS:", tot_reward) -env.close() -print("... End.") From 958a3da4e5d7230decce4d78d01dc0ee6d669c07 Mon Sep 17 00:00:00 2001 From: hunoutl Date: Mon, 9 Jan 2023 15:09:53 +0100 Subject: [PATCH 3/4] [envs] fix examples --- README.md | 14 +- sofagym/envs/BaseTemplate/BaseTemplateEnv.py | 24 +-- .../envs/BaseTemplate/BaseTemplateScene.py | 17 +- .../envs/BaseTemplate/BaseTemplateToolbox.py | 162 ++++-------------- sofagym/envs/BubbleMotion/BubbleMotion.py | 2 +- sofagym/envs/CartStem/CartStemEnv.py | 6 +- sofagym/envs/CatchTheObject/CatchTheObject.py | 3 +- sofagym/envs/SimpleMaze/SimpleMazeScene.py | 28 ++- sofagym/envs/SimpleMaze/SimpleMazeToolbox.py | 39 +++++ .../envs/StemPendulum/StemPendulumScene.py | 6 +- sofagym/envs/Trunk/TrunkScene.py | 10 +- sofagym/envs/Trunk/TrunkToolbox.py | 2 +- test_env.py | 4 +- 13 files changed, 147 insertions(+), 170 deletions(-) diff --git a/README.md b/README.md index 80b1dfc..f154942 100644 --- a/README.md +++ b/README.md @@ -95,18 +95,18 @@ The classic running of an episode is therefore: |Image|Name|Description|Status| |----------|:-------------|:-------------|:-------------| | |[BaseTemplate](sofagym/envs/BaseTemplate/) basetemplate-v0| A base template|NOT DONE| -| |[BubbleMotion](sofagym/envs/BubbleMotion/) bubblemotion-v0| |Plugin not found: "CosseratPlugin"| -| |[CartStem](sofagym/envs/CartStem/) cartstem-v0| |Plugin not found: "CosseratPlugin"| -| |[CartStemContact](sofagym/envs/CartStemContact/) cartstemcontact-v0| | Plugin not found: "CosseratPlugin"| -| |[CatchTheObject](sofagym/envs/CatchTheObject/) catchtheobject-v0| |Plugin not found: "CosseratPlugin"| +| |[BubbleMotion](sofagym/envs/BubbleMotion/) bubblemotion-v0| |OK| +| |[CartStem](sofagym/envs/CartStem/) cartstem-v0| |OK| +| |[CartStemContact](sofagym/envs/CartStemContact/) cartstemcontact-v0| |OK| +| |[CatchTheObject](sofagym/envs/CatchTheObject/) catchtheobject-v0| |OK| | |[ConcentricTubeRobot](sofagym/envs/CTR/) concentrictuberobot-v0| |OK | | |[DiamondRobot](sofagym/envs/Diamond/) diamondrobot-v0| |OSError: [Errno 36] File name too long| | |[Gripper](sofagym/envs/Gripper/) gripper-v0| The objective is to grasp a cube and bring it to a certain height. The closer the cube is to the target, the greater the reward.| OK| | |[Maze](sofagym/envs/Maze/) maze-v0| The Maze environment offers one scene of a ball navigating in a maze. The maze is attached to the tripod robot and the ball is moved by gravity by modifying the maze’s orientation. The tripod is actuated by three servomotors. Similarly to the Trunk Environment, the Maze environment has a dicrete action space of 6 actions, moving each servomotor by one increment, and could easily be extended to be continuous.|Importing your SOFA Scene Failed| -| |[MultiGait Robot](sofagym/envs/MultiGaitRobot/) multigaitrobot-v0| The multigait Softrobot has one scene. The goal is to move the robot forward in the *x* direction with the highest speed. env = gym.make("multigaitrobot-v0")|ERROR| +| |[MultiGait Robot](sofagym/envs/MultiGaitRobot/) multigaitrobot-v0| The multigait Softrobot has one scene. The goal is to move the robot forward in the *x* direction with the highest speed. env = gym.make("multigaitrobot-v0")|[ERROR] [SofaRuntime] ValueError: Object type MechanicalMatrixMapperMOR was not created The object is not in the factory. Need MOR plugin ?| | |[SimpleMaze](sofagym/envs/SimpleMaze/) simple_maze-v0| |ValueError: Object type Sphere<> was not created | -| |[StemPendulum](sofagym/envs/StemPendulum/) stempendulum-v0| |NotImplementedError: Importing your SOFA Scene Failed | -| |[Trunk](sofagym/envs/Trunk/) trunk-v0| The Trunk environment offers two scenarios. Both are based on the trunk robot. The first is to bring the trunk’s tip to a certain position. The second scenario is to manipulate a cup using the trunk to get the cup’s center of gravity in a predefined position. The Trunk is controlled by eight cables that can be contracted or extended by one unit. There are therefore 16 possible actions. The action space presented here is discrete but could easily be ex-tended to become continuous.|ERROR | +| |[StemPendulum](sofagym/envs/StemPendulum/) stempendulum-v0| |OK | +| |[Trunk](sofagym/envs/Trunk/) trunk-v0| The Trunk environment offers two scenarios. Both are based on the trunk robot. The first is to bring the trunk’s tip to a certain position. The second scenario is to manipulate a cup using the trunk to get the cup’s center of gravity in a predefined position. The Trunk is controlled by eight cables that can be contracted or extended by one unit. There are therefore 16 possible actions. The action space presented here is discrete but could easily be ex-tended to become continuous.|OK | | |[TrunkCup](sofagym/envs/TrunkCup/) trunkcup-v0| |OK| diff --git a/sofagym/envs/BaseTemplate/BaseTemplateEnv.py b/sofagym/envs/BaseTemplate/BaseTemplateEnv.py index 627006f..9c75434 100644 --- a/sofagym/envs/BaseTemplate/BaseTemplateEnv.py +++ b/sofagym/envs/BaseTemplate/BaseTemplateEnv.py @@ -1,22 +1,22 @@ # -*- coding: utf-8 -*- -"""Specific environment for the trunk (simplified). +"""Specific environment for the BaseTemplate. """ -__authors__ = "emenager" -__contact__ = "etienne.menager@ens-rennes.fr" -__version__ = "1.0.0" -__copyright__ = "(c) 2020, Inria" -__date__ = "Oct 7 2020" - -from sofagym.AbstractEnv import AbstractEnv -from sofagym.rpc_server import start_scene +__authors__ = "hunoutl" +__contact__ = "leo.hunout@inria.fr" +__version__ = "0.0.0" +__copyright__ = "(c) 2023, Inria" +__date__ = "Jan 9 2023" import os import numpy as np +from sofagym.AbstractEnv import AbstractEnv +from sofagym.rpc_server import start_scene + from gymnasium import spaces -class BaseEnv(AbstractEnv): +class BaseTemplateEnv(AbstractEnv): """Sub-class of AbstractEnv, dedicated to the trunk scene. See the class AbstractEnv for arguments and methods. @@ -24,7 +24,7 @@ class BaseEnv(AbstractEnv): # Setting a default configuration path = os.path.dirname(os.path.abspath(__file__)) metadata = {'render.modes': ['human', 'rgb_array']} - DEFAULT_CONFIG = {"scene": "Trunk", + DEFAULT_CONFIG = {"scene": "BaseTemplate", "deterministic": True, "source": [300, 0, 80], "target": [0, 0, 80], @@ -37,7 +37,7 @@ class BaseEnv(AbstractEnv): "render": 1, "save_data": False, "save_image": False, - "save_path": path + "/Results" + "/Trunk", + "save_path": path + "/Results", "planning": False, "discrete": True, "seed": None, diff --git a/sofagym/envs/BaseTemplate/BaseTemplateScene.py b/sofagym/envs/BaseTemplate/BaseTemplateScene.py index 2a1b799..3eebce6 100644 --- a/sofagym/envs/BaseTemplate/BaseTemplateScene.py +++ b/sofagym/envs/BaseTemplate/BaseTemplateScene.py @@ -1,3 +1,12 @@ +# -*- coding: utf-8 -*- +"""Create the scene +""" +__authors__ = "hunoutl" +__contact__ = "leo.hunout@inria.fr" +__version__ = "0.0.0" +__copyright__ = "(c) 2023, Inria" +__date__ = "Jan 9 2023" + import sys import pathlib @@ -97,13 +106,7 @@ def createScene(rootNode, config={"source": [-600.0, -25, 100], simulation.addObject('SparseLDLSolver', name='precond') simulation.addObject('GenericConstraintCorrection', solverName="precond") - trunk = Trunk(simulation, inverseMode=False) - rootNode.trunk = trunk - - if visu: - trunk.addVisualModel(color=[1., 1., 1., 0.8]) - trunk.fixExtremity() - + goal_mo = add_goal_node(rootNode) rootNode.addObject(rewardShaper(name="Reward", rootNode=rootNode, goalPos=config['goalPos'])) diff --git a/sofagym/envs/BaseTemplate/BaseTemplateToolbox.py b/sofagym/envs/BaseTemplate/BaseTemplateToolbox.py index 2ec7770..859826d 100644 --- a/sofagym/envs/BaseTemplate/BaseTemplateToolbox.py +++ b/sofagym/envs/BaseTemplate/BaseTemplateToolbox.py @@ -2,11 +2,11 @@ """Toolbox: compute reward, create scene, ... """ -__authors__ = "emenager" -__contact__ = "etienne.menager@ens-rennes.fr" -__version__ = "1.0.0" -__copyright__ = "(c) 2020, Inria" -__date__ = "Oct 7 2020" +__authors__ = "hunoutl" +__contact__ = "leo.hunout@inria.fr" +__version__ = "0.0.0" +__copyright__ = "(c) 2023, Inria" +__date__ = "Jan 9 2023" import numpy as np @@ -74,12 +74,9 @@ def getReward(self): The reward and the cost. """ - trunkTips = self._computeTips() - current_dist = np.linalg.norm(np.array(trunkTips)-np.array(self.goal_pos)) - reward = max((self.prev_dist - current_dist)/self.prev_dist, 0) - self.prev_dist = current_dist + reward, cost = {0,0} - return min(reward**(1/2), 1.0), current_dist + return reward, cost def update(self): """Update function. @@ -96,29 +93,6 @@ def update(self): """ - trunkTips = self._computeTips() - self.init_dist = np.linalg.norm(np.array(trunkTips)-np.array(self.goal_pos)) - self.prev_dist = self.init_dist - - def _computeTips(self): - """Compute the position of the tip. - - Parameters: - ---------- - None. - - Return: - ------ - The position of the tip. - """ - cables = self.rootNode.trunk.cables[:4] - size = len(cables) - - trunkTips = np.zeros(3) - for cable in cables: - trunkTips += cable.meca.position[-1]/size - - return trunkTips class goalSetter(Sofa.Core.Controller): @@ -213,24 +187,7 @@ def getState(rootNode): State: list of float The state of the environment/agent. """ - cs = 3 - - cables = rootNode.trunk.cables[:4] - nb_point = cables[0].meca.position.shape[0] - - points = [] - for i in range(nb_point): - point = np.zeros(3) - for cable in cables: - c = cable.meca.position[i] - point += c - point = [round(float(k), cs)/4 for k in point] - points += point - - goalPos = _getGoalPos(rootNode).tolist() - - state = points + goalPos - + state = 0 return state @@ -256,55 +213,54 @@ def getReward(root): return False, reward -def startCmd(root, action, duration): - """Initialize the command from root and action. +def action_to_command(actions, root, nb_step): + """Link between Gym action (int) and SOFA command (displacement of cables). Parameters: ---------- - rootNode: - The scene. action: int - The action. - duration: float - Duration of the animation. + The number of the action (Gym). + root: + The root of the scene. Returns: - ------ - None. - + ------- + The command. """ - num_cable, displacement = action_to_command(action) - startCmd_Trunk(root, root.trunk.cables[num_cable], displacement, duration) + incr = root.applyAction.compute_action(actions, nb_step) + return incr -def displace(cable, displacement): - """Change the value of the cable in the finger. + +def startCmd(root, actions, duration): + """Initialize the command from root and action. Parameters: ---------- - fingers: - The finger. - displacement: float - The displacement. + rootNode: + The scene. + action: int + The action. + duration: float + Duration of the animation. Returns: - ------- + ------ None. """ - cable.cable.value = [cable.cable.value[0] + displacement] + incr = action_to_command(actions, root, duration/root.dt.value + 1) + startCmd_StemPendulum(root, incr, duration) -def startCmd_Trunk(rootNode, cable, displacement, duration): +def startCmd_StemPendulum(rootNode, incr, duration): """Initialize the command. Parameters: ---------- rootNode: The scene. - cable: - The mechanical object of the cable to move. - displacement: float + incr: The elements of the commande. duration: float Duration of the animation. @@ -315,66 +271,18 @@ def startCmd_Trunk(rootNode, cable, displacement, duration): """ # Definition of the elements of the animation - def executeAnimation(cable, displacement, factor): - displace(cable, displacement) + def executeAnimation(rootNode, incr, factor): + rootNode.applyAction.apply_action(incr) # Add animation in the scene rootNode.AnimationManager.addAnimation( Animation( onUpdate=executeAnimation, - params={"cable": cable, - "displacement": displacement}, + params={"rootNode": rootNode, + "incr": incr}, duration=duration, mode="once", realTimeClock=False)) -def action_to_command(action): - """Link between Gym action (int) and SOFA command (displacement of cables). - - Parameters: - ---------- - action: int - The number of the action (Gym). - - Returns: - ------- - The command (number of the cabl and its displacement). - """ - if action == 0: - num_cable, displacement = 0, 1 - elif action == 1: - num_cable, displacement = 1, 1 - elif action == 2: - num_cable, displacement = 2, 1 - elif action == 3: - num_cable, displacement = 3, 1 - elif action == 4: - num_cable, displacement = 4, 1 - elif action == 5: - num_cable, displacement = 5, 1 - elif action == 6: - num_cable, displacement = 6, 1 - elif action == 7: - num_cable, displacement = 7, 1 - elif action == 8: - num_cable, displacement = 0, -1 - elif action == 9: - num_cable, displacement = 1, -1 - elif action == 10: - num_cable, displacement = 2, -1 - elif action == 11: - num_cable, displacement = 3, -1 - elif action == 12: - num_cable, displacement = 4, -1 - elif action == 13: - num_cable, displacement = 5, -1 - elif action == 14: - num_cable, displacement = 6, -1 - elif action == 15: - num_cable, displacement = 7, -1 - else: - raise NotImplementedError("Action must be in range 0 - 15") - - return num_cable, displacement def getPos(root): diff --git a/sofagym/envs/BubbleMotion/BubbleMotion.py b/sofagym/envs/BubbleMotion/BubbleMotion.py index 3d8583e..001315d 100644 --- a/sofagym/envs/BubbleMotion/BubbleMotion.py +++ b/sofagym/envs/BubbleMotion/BubbleMotion.py @@ -51,7 +51,7 @@ def _createBoard(self, parent): board.addObject('EulerImplicitSolver', name='odesolver') board.addObject('SparseLDLSolver', name='preconditioner', template="CompressedRowSparseMatrixd") board.addObject('MeshVTKLoader', name='loader', filename=path + "/mesh/Board_Volumetric.vtk") - board.addObject('TetrahedronSetTopologyContainer', src='@loader', name='container') + board.addObject('TetrahedronSetTopologyContainer', position="@loader.position", tetrahedra="@loader.tetrahedra", name='container') board.addObject('TetrahedronSetTopologyModifier') board.addObject('MechanicalObject', name='tetras', template='Vec3', rx=0, dz=0) board.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, diff --git a/sofagym/envs/CartStem/CartStemEnv.py b/sofagym/envs/CartStem/CartStemEnv.py index e7bdbbb..4aa9d0b 100644 --- a/sofagym/envs/CartStem/CartStemEnv.py +++ b/sofagym/envs/CartStem/CartStemEnv.py @@ -64,11 +64,11 @@ def __init__(self, config = None): def step(self, action): - obs, reward, done, info = super().step(action) + obs, reward, terminated, truncated, info = super().step(action) if abs(obs[0]) > self.config["max_move"]: - done = True + terminated = True - return obs, reward, done, info + return obs, reward, terminated, truncated, info def reset(self): """Reset simulation. diff --git a/sofagym/envs/CatchTheObject/CatchTheObject.py b/sofagym/envs/CatchTheObject/CatchTheObject.py index 6edca73..6f3aace 100644 --- a/sofagym/envs/CatchTheObject/CatchTheObject.py +++ b/sofagym/envs/CatchTheObject/CatchTheObject.py @@ -164,7 +164,8 @@ def _createExternalPart(self, parent, name, scale, rotation, translation): preconditioners='preconditioner', use_precond=True, update_step=1) external_part.addObject('MeshVTKLoader', name='loader', filename=self.path + "/mesh/Gripper_Volumetric.vtk", scale3d=scale, rotation=rotation, translation=translation) - external_part.addObject('TetrahedronSetTopologyContainer', src='@loader', name='container') + external_part.addObject('TetrahedronSetTopologyContainer', position="@loader.position", tetrahedra="@loader.tetrahedra", name='container') + external_part.addObject('TetrahedronSetTopologyModifier') external_part.addObject('MechanicalObject', name='tetras', template='Vec3', rx=0, dz=0) external_part.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', diff --git a/sofagym/envs/SimpleMaze/SimpleMazeScene.py b/sofagym/envs/SimpleMaze/SimpleMazeScene.py index a0999e0..8c1e38f 100644 --- a/sofagym/envs/SimpleMaze/SimpleMazeScene.py +++ b/sofagym/envs/SimpleMaze/SimpleMazeScene.py @@ -81,7 +81,33 @@ def createScene(root, config={"source": [0, 1000, 0], sphere.addObject("GenericConstraintCorrection", solverName='ldl') ball_mo = sphere.addObject("MechanicalObject", name="sphere_mo", template='Vec3', position=[0.0, 6.0, 2.0]) sphere.addObject("UniformMass", totalMass=1000) - sphere.addObject("Sphere", radius='2.0', name='0', color=[255, 0, 0, 255]) + #need customs sphere + + #sphere.addObject("Sphere", radius='2.0', name='0', color=[255, 0, 0, 255]) + ''' + visu = object.addChild('Visu') + visu.addObject('MeshOBJLoader', name='loader', filename=ball.obj, scale3d=1) + visu.addObject('OglModel', src='@loader', color=[255, 0, 0, 255]) + visu.addObject('RigidMapping') + + + object.addObject('GenerateRigidMass', name='mass', density=density, src=visu.loader.getLinkPath()) + object.mass.init() + translation = list(object.mass.centerToOrigin.value) + object.addObject('UniformMass', vertexMass="@mass.rigidMass") + + visu.loader.translation = translation + + if withCollision: + collision = object.addChild('Collision') + collision.addObject('MeshOBJLoader', name='loader', filename=collisionFilename, scale3d=scale) + collision.addObject('MeshTopology', src='@loader') + collision.addObject('MechanicalObject', translation=translation) + collision.addObject('TriangleCollisionModel', group = collisionGroup) + collision.addObject('LineCollisionModel', group = collisionGroup) + collision.addObject('PointCollisionModel', group = collisionGroup) + collision.addObject('RigidMapping') + ''' maze = model.addChild("maze") maze.addObject("MeshSTLLoader", name="loader", filename=path_mesh+"maze_4_coarse.stl", diff --git a/sofagym/envs/SimpleMaze/SimpleMazeToolbox.py b/sofagym/envs/SimpleMaze/SimpleMazeToolbox.py index aed7871..14cb674 100644 --- a/sofagym/envs/SimpleMaze/SimpleMazeToolbox.py +++ b/sofagym/envs/SimpleMaze/SimpleMazeToolbox.py @@ -28,6 +28,45 @@ SofaRuntime.importPlugin("SofaComponentAll") +def addRigidObject(node, filename, collisionFilename=None, position=[0,0,0,0,0,0,1], scale=[1,1,1], textureFilename='', color=[1,1,1], density=0.002, name='Object', withSolver=True, collisionGroup = 0, withCollision=True): + + if collisionFilename == None: + collisionFilename = filename + + object = node.addChild(name) + object.addObject('RequiredPlugin', name='SofaPlugins', pluginName='SofaRigid SofaLoader') + object.addObject('MechanicalObject', template='Rigid3', position=position, showObject=False, showObjectScale=5) + + if withSolver: + object.addObject('EulerImplicitSolver') + object.addObject('CGLinearSolver', tolerance=1e-5, iterations=25, threshold = 1e-5) + object.addObject('UncoupledConstraintCorrection') + + visu = object.addChild('Visu') + visu.addObject('MeshOBJLoader', name='loader', filename=filename, scale3d=scale) + visu.addObject('OglModel', src='@loader', color=color if textureFilename =='' else '') + visu.addObject('RigidMapping') + + object.addObject('GenerateRigidMass', name='mass', density=density, src=visu.loader.getLinkPath()) + object.mass.init() + translation = list(object.mass.centerToOrigin.value) + object.addObject('UniformMass', vertexMass="@mass.rigidMass") + + visu.loader.translation = translation + + if withCollision: + collision = object.addChild('Collision') + collision.addObject('MeshOBJLoader', name='loader', filename=collisionFilename, scale3d=scale) + collision.addObject('MeshTopology', src='@loader') + collision.addObject('MechanicalObject', translation=translation) + collision.addObject('TriangleCollisionModel', group = collisionGroup) + collision.addObject('LineCollisionModel', group = collisionGroup) + collision.addObject('PointCollisionModel', group = collisionGroup) + collision.addObject('RigidMapping') + + return object + + class rewardShaper(Sofa.Core.Controller): """Compute the reward. diff --git a/sofagym/envs/StemPendulum/StemPendulumScene.py b/sofagym/envs/StemPendulum/StemPendulumScene.py index d688f63..bda7037 100644 --- a/sofagym/envs/StemPendulum/StemPendulumScene.py +++ b/sofagym/envs/StemPendulum/StemPendulumScene.py @@ -21,9 +21,9 @@ sys.path.insert(0, str(pathlib.Path(__file__).parent.absolute())+"/../") sys.path.insert(0, str(pathlib.Path(__file__).parent.absolute())) -from common.header import addHeader as header -from common.header import addVisu as visu -from common.utils import addRigidObject +from sofagym.header import addHeader as header +from sofagym.header import addVisu as visu +from sofagym.utils import addRigidObject from StemPendulum import StemPendulum from StemPendulumToolbox import rewardShaper, sceneModerator, applyAction, goalSetter diff --git a/sofagym/envs/Trunk/TrunkScene.py b/sofagym/envs/Trunk/TrunkScene.py index 56d0c6c..967e3fb 100644 --- a/sofagym/envs/Trunk/TrunkScene.py +++ b/sofagym/envs/Trunk/TrunkScene.py @@ -5,11 +5,11 @@ sys.path.insert(0, str(pathlib.Path(__file__).parent.absolute())) -from splib3.animation import AnimationManagerController +from splib.animation import AnimationManagerController from math import cos, sin import numpy as np -from splib3.objectmodel import SofaPrefab, SofaObject -from splib3.numerics import Vec3, Quat +from splib.objectmodel import SofaPrefab, SofaObject +from splib.numerics import Vec3, Quat from TrunkToolbox import rewardShaper, goalSetter @@ -36,7 +36,7 @@ def effectorTarget(parentNode, position=[0., 0., 200]): return target -@SofaPrefab +#@SofaPrefab class Trunk(SofaObject): """ This prefab is implementing a soft robot inspired by the elephant's trunk. The robot is entirely soft and actuated with 8 cables. @@ -56,7 +56,7 @@ def __init__(self, parentNode, youngModulus=450, poissonRatio=0.45, totalMass=0. self.node = parentNode.addChild('Trunk') self.node.addObject('MeshVTKLoader', name='loader', filename=path+'trunk.vtk') - self.node.addObject('TetrahedronSetTopologyContainer', src='@loader', name='container') + self.node.addObject('TetrahedronSetTopologyContainer', position="@loader.position", tetrahedra="@loader.tetrahedra", name='container') self.node.addObject('TetrahedronSetTopologyModifier') self.node.addObject('TetrahedronSetGeometryAlgorithms') diff --git a/sofagym/envs/Trunk/TrunkToolbox.py b/sofagym/envs/Trunk/TrunkToolbox.py index 2ec7770..fddd95a 100644 --- a/sofagym/envs/Trunk/TrunkToolbox.py +++ b/sofagym/envs/Trunk/TrunkToolbox.py @@ -14,7 +14,7 @@ import Sofa.Core import Sofa.Simulation import SofaRuntime -from splib3.animation.animate import Animation +from splib.animation.animate import Animation SofaRuntime.importPlugin("SofaComponentAll") diff --git a/test_env.py b/test_env.py index bee3054..2881599 100644 --- a/test_env.py +++ b/test_env.py @@ -17,7 +17,7 @@ import time import gymnasium as gym -from sofagym import * +import sofagym from sofagym.envs import * RANDOM = False @@ -44,7 +44,7 @@ 13:'trunkcup-v0', } -num = 2 +num = 12 env_name = name[num] print("Start env ", env_name) From e3cbd8c0e7a02d32e9c80f05ff73ec9a133a7501 Mon Sep 17 00:00:00 2001 From: hunoutl Date: Tue, 10 Jan 2023 16:49:11 +0100 Subject: [PATCH 4/4] [envs] revert on BaseTemplate -> need PR --- README.md | 1 - sofagym/envs/BaseTemplate/BaseTemplateEnv.py | 92 ----- .../envs/BaseTemplate/BaseTemplateScene.py | 115 ------- .../envs/BaseTemplate/BaseTemplateToolbox.py | 323 ------------------ sofagym/envs/__init__.py | 5 - test_env.py | 2 +- 6 files changed, 1 insertion(+), 537 deletions(-) delete mode 100644 sofagym/envs/BaseTemplate/BaseTemplateEnv.py delete mode 100644 sofagym/envs/BaseTemplate/BaseTemplateScene.py delete mode 100644 sofagym/envs/BaseTemplate/BaseTemplateToolbox.py diff --git a/README.md b/README.md index f154942..30fc957 100644 --- a/README.md +++ b/README.md @@ -94,7 +94,6 @@ The classic running of an episode is therefore: |Image|Name|Description|Status| |----------|:-------------|:-------------|:-------------| -| |[BaseTemplate](sofagym/envs/BaseTemplate/) basetemplate-v0| A base template|NOT DONE| | |[BubbleMotion](sofagym/envs/BubbleMotion/) bubblemotion-v0| |OK| | |[CartStem](sofagym/envs/CartStem/) cartstem-v0| |OK| | |[CartStemContact](sofagym/envs/CartStemContact/) cartstemcontact-v0| |OK| diff --git a/sofagym/envs/BaseTemplate/BaseTemplateEnv.py b/sofagym/envs/BaseTemplate/BaseTemplateEnv.py deleted file mode 100644 index 9c75434..0000000 --- a/sofagym/envs/BaseTemplate/BaseTemplateEnv.py +++ /dev/null @@ -1,92 +0,0 @@ -# -*- coding: utf-8 -*- -"""Specific environment for the BaseTemplate. -""" - -__authors__ = "hunoutl" -__contact__ = "leo.hunout@inria.fr" -__version__ = "0.0.0" -__copyright__ = "(c) 2023, Inria" -__date__ = "Jan 9 2023" - -import os -import numpy as np - -from sofagym.AbstractEnv import AbstractEnv -from sofagym.rpc_server import start_scene - -from gymnasium import spaces - -class BaseTemplateEnv(AbstractEnv): - """Sub-class of AbstractEnv, dedicated to the trunk scene. - - See the class AbstractEnv for arguments and methods. - """ - # Setting a default configuration - path = os.path.dirname(os.path.abspath(__file__)) - metadata = {'render.modes': ['human', 'rgb_array']} - DEFAULT_CONFIG = {"scene": "BaseTemplate", - "deterministic": True, - "source": [300, 0, 80], - "target": [0, 0, 80], - "goalList": [[40, 40, 100], [-10, 20, 80]], - "start_node": None, - "scale_factor": 5, - "timer_limit": 250, - "timeout": 50, - "display_size": (1600, 800), - "render": 1, - "save_data": False, - "save_image": False, - "save_path": path + "/Results", - "planning": False, - "discrete": True, - "seed": None, - "start_from_history": None, - "python_version": "python3" - } - - def __init__(self, config = None): - super().__init__(config) - nb_actions = 2 - self.action_space = spaces.Discrete(nb_actions) - self.nb_actions = str(nb_actions) - - dim_state = 10 - 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 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. - - """ - super().reset() - self.config.update({'goalPos': self.goal}) - obs = start_scene(self.config, self.nb_actions) - - return 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 list(range(int(self.nb_actions))) - - - diff --git a/sofagym/envs/BaseTemplate/BaseTemplateScene.py b/sofagym/envs/BaseTemplate/BaseTemplateScene.py deleted file mode 100644 index 3eebce6..0000000 --- a/sofagym/envs/BaseTemplate/BaseTemplateScene.py +++ /dev/null @@ -1,115 +0,0 @@ -# -*- coding: utf-8 -*- -"""Create the scene -""" -__authors__ = "hunoutl" -__contact__ = "leo.hunout@inria.fr" -__version__ = "0.0.0" -__copyright__ = "(c) 2023, Inria" -__date__ = "Jan 9 2023" - -import sys -import pathlib - -sys.path.insert(0, str(pathlib.Path(__file__).parent.absolute())+"/../") -sys.path.insert(0, str(pathlib.Path(__file__).parent.absolute())) - - -from splib3.animation import AnimationManagerController -from math import cos, sin -import numpy as np -from splib3.objectmodel import SofaPrefab, SofaObject -from splib3.numerics import Vec3, Quat - - -from sofagym.envs.BaseTemplate.BaseTemplateToolbox import rewardShaper, goalSetter - -import os -path = os.path.dirname(os.path.abspath(__file__))+'/mesh/' - - -def add_goal_node(root): - goal = root.addChild("Goal") - goal.addObject('VisualStyle', displayFlags="showCollisionModels") - goal_mo = goal.addObject('MechanicalObject', name='GoalMO', showObject=True, drawMode="1", showObjectScale=3, - showColor=[0, 1, 0, 1], position=[0.0, -100.0, 100.0]) - return goal_mo - - -def effectorTarget(parentNode, position=[0., 0., 200]): - target = parentNode.addChild("Target") - target.addObject("EulerImplicitSolver", firstOrder=True) - target.addObject("CGLinearSolver") - target.addObject("MechanicalObject", name="dofs", position=position, showObject=True, showObjectScale=3, - drawMode=2, showColor=[1., 1., 1., 1.]) - target.addObject("UncoupledConstraintCorrection") - return target - - -def createScene(rootNode, config={"source": [-600.0, -25, 100], - "target": [30, -25, 100], - "goalPos": [0, 0, 0]}, mode='simu_and_visu'): - - # Chose the mode: visualization or computations (or both) - visu, simu = False, False - if 'visu' in mode: - visu = True - if 'simu' in mode: - simu = True - - rootNode.addObject("RequiredPlugin", name="SoftRobots") - rootNode.addObject("RequiredPlugin", name="SofaSparseSolver") - rootNode.addObject("RequiredPlugin", name="SofaPreconditioner") - rootNode.addObject("RequiredPlugin", name="SofaPython3") - rootNode.addObject('RequiredPlugin', name='BeamAdapter') - rootNode.addObject('RequiredPlugin', name='SofaOpenglVisual') - rootNode.addObject('RequiredPlugin', name="SofaMiscCollision") - rootNode.addObject("RequiredPlugin", name="SofaBoundaryCondition") - rootNode.addObject("RequiredPlugin", name="SofaConstraint") - rootNode.addObject("RequiredPlugin", name="SofaEngine") - rootNode.addObject('RequiredPlugin', name='SofaImplicitOdeSolver') - rootNode.addObject('RequiredPlugin', name='SofaLoader') - rootNode.addObject('RequiredPlugin', name="SofaSimpleFem") - - if visu: - source = config["source"] - target = config["target"] - rootNode.addObject('VisualStyle', displayFlags='showVisualModels hideBehaviorModels hideCollisionModels ' - 'hideMappings hideForceFields showWireframe') - rootNode.addObject("LightManager") - - spotLoc = [2*source[0], 0, 0] - rootNode.addObject("SpotLight", position=spotLoc, direction=[-np.sign(source[0]), 0.0, 0.0]) - rootNode.addObject("InteractiveCamera", name='camera', position=source, lookAt=target, zFar=500) - rootNode.addObject('BackgroundSetting', color=[1, 1, 1, 1]) - if simu: - rootNode.addObject('DefaultPipeline') - rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('GenericConstraintSolver', tolerance="1e-6", maxIterations="1000") - rootNode.addObject('BruteForceDetection') - rootNode.addObject('RuleBasedContactManager', responseParams="mu="+str(0.3), name='Response', - response='FrictionContactConstraint') - rootNode.addObject('LocalMinDistance', alarmDistance=10, contactDistance=5, angleCone=0.01) - - rootNode.addObject(AnimationManagerController(name="AnimationManager")) - - rootNode.gravity.value = [0., -9810., 0.] - - rootNode.dt.value = 0.01 - - simulation = rootNode.addChild("Simulation") - - if simu: - simulation.addObject('EulerImplicitSolver', name='odesolver', firstOrder="0", rayleighMass="0.1", - rayleighStiffness="0.1") - simulation.addObject('ShewchukPCGLinearSolver', name='linearSolver', iterations='500', tolerance='1.0e-18', - preconditioners="precond") - simulation.addObject('SparseLDLSolver', name='precond') - simulation.addObject('GenericConstraintCorrection', solverName="precond") - - - goal_mo = add_goal_node(rootNode) - - rootNode.addObject(rewardShaper(name="Reward", rootNode=rootNode, goalPos=config['goalPos'])) - rootNode.addObject(goalSetter(name="GoalSetter", goalMO=goal_mo, goalPos=config['goalPos'])) - - return rootNode diff --git a/sofagym/envs/BaseTemplate/BaseTemplateToolbox.py b/sofagym/envs/BaseTemplate/BaseTemplateToolbox.py deleted file mode 100644 index 859826d..0000000 --- a/sofagym/envs/BaseTemplate/BaseTemplateToolbox.py +++ /dev/null @@ -1,323 +0,0 @@ -# -*- coding: utf-8 -*- -"""Toolbox: compute reward, create scene, ... -""" - -__authors__ = "hunoutl" -__contact__ = "leo.hunout@inria.fr" -__version__ = "0.0.0" -__copyright__ = "(c) 2023, Inria" -__date__ = "Jan 9 2023" - -import numpy as np - -import Sofa -import Sofa.Core -import Sofa.Simulation -import SofaRuntime -from splib3.animation.animate import Animation - -SofaRuntime.importPlugin("SofaComponentAll") - - -class rewardShaper(Sofa.Core.Controller): - """Compute the reward. - - Methods: - ------- - __init__: Initialization of all arguments. - getReward: Compute the reward. - update: Initialize the value of cost. - - Arguments: - --------- - rootNode: - The scene. - goal_pos: coordinates - The position of the goal. - effMO: - The mechanical object of the element to move. - cost: - Evolution of the distance between object and goal. - - """ - def __init__(self, *args, **kwargs): - """Initialization of all arguments. - - Parameters: - ---------- - kwargs: Dictionary - Initialization of the arguments. - - Returns: - ------- - None. - - """ - Sofa.Core.Controller.__init__(self, *args, **kwargs) - - self.rootNode = None - if kwargs["rootNode"]: - self.rootNode = kwargs["rootNode"] - self.goal_pos = None - if kwargs["goalPos"]: - self.goal_pos = kwargs["goalPos"] - - def getReward(self): - """Compute the reward. - - Parameters: - ---------- - None. - - Returns: - ------- - The reward and the cost. - - """ - reward, cost = {0,0} - - return reward, cost - - def update(self): - """Update function. - - This function is used as an initialization function. - - Parameters: - ---------- - None. - - Arguments: - --------- - None. - - """ - - - -class goalSetter(Sofa.Core.Controller): - """Compute the goal. - - Methods: - ------- - __init__: Initialization of all arguments. - update: Initialize the value of cost. - - Arguments: - --------- - goalMO: - The mechanical object of the goal. - goalPos: coordinates - The coordinates of the goal. - - """ - - def __init__(self, *args, **kwargs): - """Initialization of all arguments. - - Parameters: - ---------- - kwargs: Dictionary - Initialization of the arguments. - - Returns: - ------- - None. - - """ - Sofa.Core.Controller.__init__(self, *args, **kwargs) - - self.goalMO = None - if kwargs["goalMO"]: - self.goalMO = kwargs["goalMO"] - self.goalPos = None - if kwargs["goalPos"]: - self.goalPos = kwargs["goalPos"] - - def update(self): - """Set the position of the goal. - - This function is used as an initialization function. - - Parameters: - ---------- - None. - - Arguments: - --------- - None. - - """ - with self.goalMO.position.writeable() as position: - position += self.goalPos - - def set_mo_pos(self, goal): - """Modify the goal. - - Not used here. - """ - pass - - -def _getGoalPos(rootNode): - """Get XYZ position of the goal. - - Parameters: - ---------- - rootNode: - The scene. - - Returns: - ------- - The position of the goal. - """ - return rootNode.Goal.GoalMO.position[0] - - -def getState(rootNode): - """Compute the state of the environment/agent. - - Parameters: - ---------- - rootNode: - The scene. - - Returns: - ------- - State: list of float - The state of the environment/agent. - """ - state = 0 - return state - - -def getReward(root): - """Compute the reward using Reward.getReward(). - - Parameters: - ---------- - rootNode: - The scene. - - Returns: - ------- - done, reward - - """ - - reward, cost = root.Reward.getReward() - - if cost <= 1.0: - return True, reward - - return False, reward - - -def action_to_command(actions, root, nb_step): - """Link between Gym action (int) and SOFA command (displacement of cables). - - Parameters: - ---------- - action: int - The number of the action (Gym). - root: - The root of the scene. - - Returns: - ------- - The command. - """ - - incr = root.applyAction.compute_action(actions, nb_step) - return incr - - -def startCmd(root, actions, duration): - """Initialize the command from root and action. - - Parameters: - ---------- - rootNode: - The scene. - action: int - The action. - duration: float - Duration of the animation. - - Returns: - ------ - None. - - """ - incr = action_to_command(actions, root, duration/root.dt.value + 1) - startCmd_StemPendulum(root, incr, duration) - - -def startCmd_StemPendulum(rootNode, incr, duration): - """Initialize the command. - - Parameters: - ---------- - rootNode: - The scene. - incr: - The elements of the commande. - duration: float - Duration of the animation. - - Returns: - ------- - None. - """ - - # Definition of the elements of the animation - def executeAnimation(rootNode, incr, factor): - rootNode.applyAction.apply_action(incr) - - # Add animation in the scene - rootNode.AnimationManager.addAnimation( - Animation( - onUpdate=executeAnimation, - params={"rootNode": rootNode, - "incr": incr}, - duration=duration, mode="once", realTimeClock=False)) - - - - -def getPos(root): - """Retun the position of the mechanical object of interest. - - Parameters: - ---------- - root: - The root of the scene. - - Returns: - ------- - _: list - The position(s) of the object(s) of the scene. - """ - return root.Simulation.Trunk.dofs.position.value.tolist() - - -def setPos(root, pos): - """Set the position of the mechanical object of interest. - - Parameters: - ---------- - root: - The root of the scene. - pos: list - The position(s) of the object(s) of the scene. - - Returns: - ------- - None. - - Note: - ---- - Don't forget to init the new value of the position. - - """ - root.Simulation.Trunk.dofs.position.value = np.array(pos) diff --git a/sofagym/envs/__init__.py b/sofagym/envs/__init__.py index 1286f4d..ac67982 100644 --- a/sofagym/envs/__init__.py +++ b/sofagym/envs/__init__.py @@ -1,4 +1,3 @@ -from sofagym.envs.BaseTemplate.BaseTemplateEnv import * from sofagym.envs.BubbleMotion.BubbleMotionEnv import * from sofagym.envs.CartStem.CartStemEnv import * from sofagym.envs.CartStemContact.CartStemContactEnv import * @@ -16,10 +15,6 @@ # registering sofagym envs as gymnasium envs from gymnasium.envs.registration import register -register( - id='basetemplate-v0', - entry_point='sofagym.envs:BaseTemplateEnv', -) register( id='bubblemotion-v0', entry_point='sofagym.envs:BubbleMotionEnv', diff --git a/test_env.py b/test_env.py index 2881599..390d655 100644 --- a/test_env.py +++ b/test_env.py @@ -28,7 +28,7 @@ sys.path.insert(0, os.getcwd()+"/..") __import__('sofagym') -name = {0:'basetemplate-v0', +name = { 1:'bubblemotion-v0', 2:'cartstem-v0', 3:'cartstemcontact-v0',