├── __init__.py ├── raisimGymTorch ├── algo │ ├── __init__.py │ └── ppo │ │ ├── __init__.py │ │ ├── storage.py │ │ ├── dagger.py │ │ ├── ppo.py │ │ └── module.py ├── env │ ├── __init__.py │ ├── bin │ │ ├── __init__.py │ │ ├── rsg_anymal.exp │ │ └── rsg_anymal.lib │ ├── envs │ │ ├── .gitignore │ │ ├── rsg_a1_task │ │ │ ├── cfg.yaml │ │ │ ├── viz_policy.py │ │ │ ├── evaluate_policy.py │ │ │ └── runner.py │ │ └── dagger_a1 │ │ │ ├── cfg.yaml │ │ │ ├── viz_policy.py │ │ │ ├── evaluate_policy.py │ │ │ └── dagger.py │ ├── RewardLogger.hpp │ ├── debug_app.cpp │ ├── raisim_gym.cpp │ ├── RaisimGymEnv.hpp │ ├── VectorizedEnvironment.hpp │ ├── RaisimGymVecEnv.py │ └── Yaml.hpp └── helper │ ├── __init__.py │ └── raisim_gym_helper.py ├── data └── base_policy │ ├── full_22000.pt │ └── policy_22000.pt ├── .gitignore ├── eval_scripts └── compute_results.py ├── setup.py ├── CMakeLists.txt ├── README.md └── LICENSE.md /__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /raisimGymTorch/algo/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /raisimGymTorch/env/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /raisimGymTorch/env/bin/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /raisimGymTorch/helper/__init__.py: -------------------------------------------------------------------------------- 1 | from .raisim_gym_helper import * 2 | -------------------------------------------------------------------------------- /data/base_policy/full_22000.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/antonilo/rl_locomotion/HEAD/data/base_policy/full_22000.pt -------------------------------------------------------------------------------- /data/base_policy/policy_22000.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/antonilo/rl_locomotion/HEAD/data/base_policy/policy_22000.pt -------------------------------------------------------------------------------- /raisimGymTorch/env/bin/rsg_anymal.exp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/antonilo/rl_locomotion/HEAD/raisimGymTorch/env/bin/rsg_anymal.exp -------------------------------------------------------------------------------- /raisimGymTorch/env/bin/rsg_anymal.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/antonilo/rl_locomotion/HEAD/raisimGymTorch/env/bin/rsg_anymal.lib -------------------------------------------------------------------------------- /raisimGymTorch/algo/ppo/__init__.py: -------------------------------------------------------------------------------- 1 | from .ppo import PPO 2 | from .storage import RolloutStorage 3 | from .module import Actor, Critic 4 | -------------------------------------------------------------------------------- /raisimGymTorch/env/envs/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in repository root 2 | /* 3 | 4 | # Files to not ignore 5 | !/.gitignore 6 | !/some_other_files 7 | 8 | # Folder to not ignore 9 | !/rsg*/ 10 | !/a1_basic/ 11 | !/a1_deploy/ 12 | !/a1_general/ 13 | !/a1_model_based/ 14 | !/vision_a1/ 15 | !/dagger_a1/ 16 | !/sysid_a1/ 17 | !/arma_a1/ 18 | 19 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | data 3 | dist/ 4 | _build/ 5 | _generate/ 6 | *.so 7 | *.py[cod] 8 | *.egg-info 9 | .idea/* 10 | *.prof 11 | cmake-*/* 12 | *.log 13 | *imgui.ini 14 | raisimGymTorch/env/data/* 15 | .vscode/* 16 | id_rsa 17 | */data/* 18 | log_* 19 | raisimGymTorch/env/bin/*_debug_app 20 | *.png 21 | *.zip 22 | .DS_Store 23 | runs*/ 24 | plots/ 25 | data_rss/ 26 | -------------------------------------------------------------------------------- /eval_scripts/compute_results.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pandas as pd 3 | import sys 4 | import copy 5 | from prettytable import PrettyTable 6 | 7 | path = sys.argv[1] 8 | max_steps = 1100 9 | dividers = ['forward_r', 'energy', 'smoothness', 'ground_impact'] 10 | 11 | df = pd.read_csv(path) 12 | num_steps = copy.deepcopy(df['num_steps'].values) 13 | df['num_steps'] /= max_steps 14 | df['success_rate'] = df['num_steps'] > 0.99 15 | 16 | print("Num experiments is {}".format(df.shape[0])) 17 | 18 | names = df.columns.tolist() 19 | medians = [] 20 | 21 | for n in names: 22 | if n in dividers: 23 | df[n] /= num_steps 24 | value = np.mean(df[n].values) 25 | medians.append(value) 26 | 27 | t = PrettyTable(names) 28 | t.add_row(medians) 29 | print(t) 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /raisimGymTorch/env/RewardLogger.hpp: -------------------------------------------------------------------------------- 1 | //----------------------------// 2 | // This file is part of RaiSim// 3 | // Copyright 2020, RaiSim Tech// 4 | //----------------------------// 5 | 6 | #ifndef _RAISIM_GYM_REWARDLOGGER_HPP 7 | #define _RAISIM_GYM_REWARDLOGGER_HPP 8 | 9 | #include 10 | 11 | namespace raisim { 12 | 13 | class RewardLogger { 14 | 15 | class RewardTerm { 16 | public: 17 | RewardTerm() = default; 18 | 19 | void clean() { 20 | sum = 0.; 21 | count = 0; 22 | values.clear(); 23 | } 24 | 25 | void log(double value) { 26 | values.push_back(value); 27 | sum += value; 28 | count++; 29 | } 30 | 31 | double sum = 0.; 32 | int count = 0; 33 | std::vector values; 34 | }; 35 | 36 | public: 37 | RewardLogger() = default; 38 | 39 | void init(std::initializer_list rewardTermNames) { 40 | for (auto &item : rewardTermNames) 41 | rewardTerms_[item] = RewardTerm(); 42 | } 43 | 44 | void log(const std::string &termName, double value) { 45 | rewardTerms_[termName].log(value); 46 | } 47 | 48 | const std::unordered_map& getRewardTerms() const { 49 | return rewardTerms_; 50 | } 51 | 52 | void clean() { 53 | for (auto& item : rewardTerms_) 54 | item.second.clean(); 55 | } 56 | 57 | private: 58 | std::unordered_map rewardTerms_; 59 | }; 60 | 61 | }; 62 | #endif //_RAISIM_GYM_REWARDLOGGER_HPP 63 | -------------------------------------------------------------------------------- /raisimGymTorch/helper/raisim_gym_helper.py: -------------------------------------------------------------------------------- 1 | from ruamel.yaml import dump, RoundTripDumper 2 | from shutil import copyfile 3 | import datetime 4 | import os 5 | import ntpath 6 | 7 | #def generate_string_from_config(config): 8 | # float_vals = [] 9 | # for k in config['environment'].keys(): 10 | # if k.endswith('Coeff'): 11 | # float_vals.append(config['environment'][k]) 12 | # float_vals = [str(v) for v in float_vals] 13 | # return '_'.join(float_vals) 14 | 15 | class ConfigurationSaver: 16 | def __init__(self, log_dir, save_items, config = None, overwrite = False): 17 | if config is not None: 18 | self._data_dir = log_dir + '/' #+ generate_string_from_config(config) 19 | else: 20 | self._data_dir = log_dir + '/' + datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S") 21 | os.makedirs(self._data_dir, exist_ok = overwrite) 22 | 23 | if config is not None: 24 | with open(self._data_dir + '/cfg.yaml', 'w') as fptr: 25 | dump(config, stream = fptr, Dumper = RoundTripDumper) 26 | 27 | if save_items is not None: 28 | for save_item in save_items: 29 | base_file_name = ntpath.basename(save_item) 30 | copyfile(save_item, self._data_dir + '/' + base_file_name) 31 | 32 | @property 33 | def data_dir(self): 34 | return self._data_dir 35 | 36 | 37 | def TensorboardLauncher(directory_path): 38 | from tensorboard import program 39 | import webbrowser 40 | # learning visualizer 41 | tb = program.TensorBoard() 42 | tb.configure(argv=[None, '--logdir', directory_path]) 43 | url = tb.launch() 44 | print("[RAISIM_GYM] Tensorboard session created: "+url) 45 | webbrowser.open_new(url) 46 | -------------------------------------------------------------------------------- /raisimGymTorch/env/debug_app.cpp: -------------------------------------------------------------------------------- 1 | //----------------------------// 2 | // This file is part of RaiSim// 3 | // Copyright 2020, RaiSim Tech// 4 | //----------------------------// 5 | 6 | #include "Environment.hpp" 7 | #include "VectorizedEnvironment.hpp" 8 | 9 | using namespace raisim; 10 | 11 | int main(int argc, char *argv[]) { 12 | RSFATAL_IF(argc != 3, "got "< vecEnv(resourceDir, config_str); 36 | vecEnv.init(); 37 | 38 | Yaml::Node config; 39 | Yaml::Parse(config, config_str); 40 | 41 | EigenRowMajorMat observation(config["num_envs"].template As(), vecEnv.getObDim()); 42 | EigenRowMajorMat action(config["num_envs"].template As(), vecEnv.getActionDim()); 43 | EigenVec reward(config["num_envs"].template As(), 1); 44 | EigenBoolVec dones(config["num_envs"].template As(), 1); 45 | action.setZero(); 46 | 47 | //Eigen::Ref ob_ref(observation), action_ref(action); 48 | Eigen::Ref ob_ref(observation); 49 | Eigen::Ref reward_ref(reward); 50 | Eigen::Ref dones_ref(dones); 51 | 52 | for(int i =0; i< 10; i++){ 53 | vecEnv.reset(); 54 | for(int j = 0; j < 400; j++){ 55 | action = EigenRowMajorMat::Random(config["num_envs"].template As(), vecEnv.getActionDim()); 56 | Eigen::Ref action_ref(action); 57 | vecEnv.step(action_ref, reward_ref, dones_ref); 58 | } 59 | } 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /raisimGymTorch/env/envs/rsg_a1_task/cfg.yaml: -------------------------------------------------------------------------------- 1 | seed: 1 2 | record_video: yes 3 | environment: 4 | use_priv_vel: true 5 | use_slope_dots: true 6 | render: false 7 | num_envs: 200 8 | eval_every_n: 2000 9 | num_threads: 20 10 | baseDim: 42 11 | geomDim: 2 12 | n_futures: 1 13 | simulation_dt: 0.0025 14 | control_dt: 0.01 15 | max_time: 6.0 16 | pid_coeff: 45.0 17 | slope_threshold: 0.08 18 | forwardVelRewardCoeff: 60 19 | lateralVelRewardCoeff: 20 20 | angularVelRewardCoeff: 35 21 | torqueRewardCoeff: 0.0 22 | deltaTorqueRewardCoeff: -0.0 23 | actionRewardCoeff: -0.05 24 | sidewaysRewardCoeff: -0.03 25 | jointSpeedRewardCoeff: -0.0007 26 | deltaContactRewardCoeff: -0.0 27 | deltaReleaseRewardCoeff: -0.0 28 | contactRewardCoeff: 0.0 29 | contactDistRewardCoeff: 0.0 30 | footSlipRewardCoeff: -0.2 31 | footClearenceRewardCoeff: 0.0 32 | contactChangeRewardCoeff: 0.0 33 | upwardRewardCoeff: -0. 34 | workRewardCoeff: -0.05 35 | yAccRewardCoeff: -0.0 36 | max_speed: 0.4 37 | lat_speed: 0.3 38 | ang_speed: 0.6 39 | alive_bonus: 18 40 | cost_coeff: 1.0 41 | cost_decay_fac: 0.997 42 | roughTerrain: true 43 | terrainFreq: 1 44 | dynNoise: 0.0 45 | privinfo: true 46 | includeGRF: false 47 | test: false 48 | randomize_friction: true 49 | randomize_mass: true 50 | randomize_motor_strength: true 51 | randomize_gains: true 52 | randomize_yaw: false 53 | fourier: false 54 | regular_dim: 46 55 | regular_fourier_dim: 128 56 | privy_fourier_dim: 2 57 | encoder_dim: 1 58 | fourier_scale: 50 59 | fourier_trainable: true 60 | fourier_value: true 61 | fourier_policy: true 62 | cts_target_speed: false 63 | target_speed_period: 1000 64 | target_end_speed: 3.0 65 | target_start_speed: 0.0 66 | observe_base_speed: false 67 | bodyLinearVel_avg_weight: 0.2 68 | target_speed_curriculum: false 69 | unnormalize_speed_vec: false 70 | speedTest: false 71 | architecture: 72 | policy_net: [128, 128] 73 | value_net: [128, 128] 74 | freeze_encoder: false 75 | activation: tanh 76 | small_init: false 77 | speed_vec_start_idx: 42 78 | speed_vec_end_idx: 46 79 | layer_type: feedforward 80 | film_init_std: 0.1 81 | test: false 82 | -------------------------------------------------------------------------------- /raisimGymTorch/env/envs/dagger_a1/cfg.yaml: -------------------------------------------------------------------------------- 1 | seed: 1 2 | record_video: yes 3 | environment: 4 | use_priv_vel: true 5 | use_slope_dots: true 6 | render: false 7 | num_envs: 200 8 | eval_every_n: 400 9 | num_threads: 20 10 | baseDim: 42 11 | geomDim: 2 12 | n_futures: 1 13 | simulation_dt: 0.0025 14 | control_dt: 0.01 15 | max_time: 6.0 16 | pid_coeff: 45.0 17 | slope_threshold: 0.08 18 | forwardVelRewardCoeff: 60 19 | lateralVelRewardCoeff: 20 20 | angularVelRewardCoeff: 35 21 | torqueRewardCoeff: 0.0 22 | deltaTorqueRewardCoeff: -0.0 23 | actionRewardCoeff: -0.05 24 | sidewaysRewardCoeff: -0.03 25 | jointSpeedRewardCoeff: -0.0007 26 | deltaContactRewardCoeff: -0.0 27 | deltaReleaseRewardCoeff: -0.0 28 | contactRewardCoeff: 0.0 29 | contactDistRewardCoeff: 0.0 30 | footSlipRewardCoeff: -0.2 31 | footClearenceRewardCoeff: 0.0 32 | contactChangeRewardCoeff: 0.0 33 | upwardRewardCoeff: -0. 34 | workRewardCoeff: -0.05 35 | yAccRewardCoeff: -0.0 36 | max_speed: 0.4 37 | lat_speed: 0.3 38 | ang_speed: 0.6 39 | alive_bonus: 18 40 | cost_coeff: 1.0 41 | cost_decay_fac: 0.997 42 | roughTerrain: true 43 | terrainFreq: 1 44 | dynNoise: 0.0 45 | privinfo: true 46 | includeGRF: false 47 | test: false 48 | randomize_friction: true 49 | randomize_mass: true 50 | randomize_motor_strength: true 51 | randomize_gains: true 52 | randomize_yaw: false 53 | fourier: false 54 | regular_dim: 46 55 | regular_fourier_dim: 128 56 | privy_fourier_dim: 2 57 | encoder_dim: 1 58 | fourier_scale: 50 59 | fourier_trainable: true 60 | fourier_value: true 61 | fourier_policy: true 62 | cts_target_speed: false 63 | target_speed_period: 1000 64 | target_end_speed: 3.0 65 | target_start_speed: 0.0 66 | observe_base_speed: false 67 | bodyLinearVel_avg_weight: 0.2 68 | target_speed_curriculum: false 69 | unnormalize_speed_vec: false 70 | speedTest: false 71 | history_len: 50 72 | architecture: 73 | policy_net: [128, 128] 74 | value_net: [128, 128] 75 | freeze_encoder: false 76 | activation: tanh 77 | small_init: false 78 | speed_vec_start_idx: 42 79 | speed_vec_end_idx: 46 80 | layer_type: feedforward 81 | film_init_std: 0.1 82 | test: false 83 | -------------------------------------------------------------------------------- /raisimGymTorch/env/raisim_gym.cpp: -------------------------------------------------------------------------------- 1 | //----------------------------// 2 | // This file is part of RaiSim// 3 | // Copyright 2020, RaiSim Tech// 4 | //----------------------------// 5 | 6 | #include 7 | #include 8 | #include 9 | #include "Environment.hpp" 10 | #include "VectorizedEnvironment.hpp" 11 | 12 | namespace py = pybind11; 13 | using namespace raisim; 14 | 15 | #ifndef ENVIRONMENT_NAME 16 | #define ENVIRONMENT_NAME RaisimGymEnv 17 | #endif 18 | 19 | PYBIND11_MODULE(RAISIMGYM_TORCH_ENV_NAME, m) { 20 | py::class_>(m, RSG_MAKE_STR(ENVIRONMENT_NAME)) 21 | .def(py::init()) 22 | .def("init", &VectorizedEnvironment::init) 23 | .def("reset", &VectorizedEnvironment::reset) 24 | .def("observe", &VectorizedEnvironment::observe) 25 | .def("step", &VectorizedEnvironment::step) 26 | .def("setSeed", &VectorizedEnvironment::setSeed) 27 | .def("close", &VectorizedEnvironment::close) 28 | .def("isTerminalState", &VectorizedEnvironment::isTerminalState) 29 | .def("setSimulationTimeStep", &VectorizedEnvironment::setSimulationTimeStep) 30 | .def("setControlTimeStep", &VectorizedEnvironment::setControlTimeStep) 31 | .def("setItrNumber", &VectorizedEnvironment::setItrNumber) 32 | .def("getObDim", &VectorizedEnvironment::getObDim) 33 | .def("getActionDim", &VectorizedEnvironment::getActionDim) 34 | .def("getNumOfEnvs", &VectorizedEnvironment::getNumOfEnvs) 35 | .def("turnOnVisualization", &VectorizedEnvironment::turnOnVisualization) 36 | .def("turnOffVisualization", &VectorizedEnvironment::turnOffVisualization) 37 | .def("stopRecordingVideo", &VectorizedEnvironment::stopRecordingVideo) 38 | .def("startRecordingVideo", &VectorizedEnvironment::startRecordingVideo) 39 | .def("curriculumUpdate", &VectorizedEnvironment::curriculumUpdate) 40 | .def("getDis", &VectorizedEnvironment::getDis) 41 | .def("getRewardInfo", &VectorizedEnvironment::getRewardInfo); 42 | } 43 | -------------------------------------------------------------------------------- /raisimGymTorch/env/envs/rsg_a1_task/viz_policy.py: -------------------------------------------------------------------------------- 1 | from ruamel.yaml import YAML, dump, RoundTripDumper 2 | from raisimGymTorch.env.bin import rsg_a1_task 3 | from raisimGymTorch.env.RaisimGymVecEnv import RaisimGymVecEnv as VecEnv 4 | import os 5 | import math 6 | import time 7 | import numpy as np 8 | import torch 9 | import sys 10 | np.set_printoptions(suppress=True, precision=3) 11 | 12 | # directories 13 | task_path = os.path.dirname(os.path.realpath(__file__)) 14 | home_path = task_path + "/../../../../.." 15 | base_dir = sys.argv[1] 16 | runid = sys.argv[2] 17 | 18 | # config 19 | cfg = YAML().load(open(sys.argv[1] + "/cfg.yaml", 'r')) 20 | cfg['environment']['num_envs'] = 1 21 | cfg['environment']['num_threads'] = 1 22 | cfg['environment']['render'] = True 23 | 24 | cfg['environment']['test'] = True 25 | # Uncomment this for more controlled tests 26 | #cfg['environment']['randomize_friction'] = False 27 | #cfg['environment']['randomize_mass'] = False 28 | #cfg['environment']['randomize_motor_strength'] = False 29 | #cfg['environment']['randomize_gains'] = False 30 | #cfg['environment']['speedTest'] = False 31 | 32 | # create environment from the configuration file 33 | env = VecEnv(rsg_a1_task.RaisimGymEnv(home_path + "/rsc", dump(cfg['environment'], Dumper=RoundTripDumper)), cfg['environment']) 34 | 35 | # shortcuts 36 | ob_dim = env.num_obs 37 | act_dim = env.num_acts 38 | 39 | # Training 40 | n_steps = math.floor(cfg['environment']['max_time'] / cfg['environment']['control_dt']) 41 | policy_load_path = '/'.join([base_dir, 'policy_' + runid + '.pt']) 42 | env.load_scaling(base_dir, int(runid)) 43 | loaded_graph = torch.jit.load(policy_load_path) 44 | 45 | 46 | foot_contacts = [] 47 | eplen = 0 48 | 49 | print("Visualizing and evaluating the current policy") 50 | for update in range(1): 51 | env.reset() 52 | env.turn_on_visualization() 53 | 54 | eplen = 0 55 | # An high number, assumes curriculum is finished 56 | env.set_itr_number(30000) #int(runid)) 57 | for step in range(50000): 58 | time.sleep(0.01) 59 | obs = env.observe(False) 60 | with torch.no_grad(): 61 | action_ll = loaded_graph(torch.from_numpy(obs).cpu()) 62 | action = action_ll.cpu().detach().numpy() 63 | 64 | reward_ll, dones = env.step(action) 65 | reward_info = env.get_reward_info()[0] 66 | eplen+=1 67 | if dones[0]: 68 | eplen = 0 69 | # You can put plotting stuff here! 70 | 71 | env.turn_off_visualization() 72 | -------------------------------------------------------------------------------- /raisimGymTorch/env/envs/dagger_a1/viz_policy.py: -------------------------------------------------------------------------------- 1 | from ruamel.yaml import YAML, dump, RoundTripDumper 2 | from raisimGymTorch.env.bin import dagger_a1 3 | from raisimGymTorch.env.RaisimGymVecEnv import RaisimGymVecEnv as VecEnv 4 | import os 5 | import math 6 | import time 7 | import torch 8 | import sys 9 | 10 | base_dir = sys.argv[1] 11 | runid = sys.argv[2] 12 | 13 | draw_plots = False 14 | use_expert_gamma = False 15 | 16 | # directories 17 | task_path = os.path.dirname(os.path.realpath(__file__)) 18 | home_path = task_path + "/../../../../.." 19 | 20 | # config 21 | cfg = YAML().load(open(sys.argv[1] + "/cfg.yaml", 'r')) 22 | cfg['environment']['num_envs'] = 1 23 | cfg['environment']['num_threads'] = 1 24 | cfg['environment']['render'] = True 25 | 26 | #cfg['environment']['randomize_friction'] = False 27 | #cfg['environment']['randomize_mass'] = False 28 | #cfg['environment']['randomize_motor_strength'] = False 29 | #cfg['environment']['randomize_gains'] = False 30 | #cfg['environment']['speedTest'] = False 31 | 32 | base_dims = cfg['environment']['baseDim'] 33 | n_futures = cfg['environment']['n_futures'] 34 | cfg['environment']['test'] = True 35 | t_steps = cfg['environment']['history_len'] 36 | 37 | # create environment from the configuration file 38 | env = VecEnv(dagger_a1.RaisimGymEnv(home_path + "/rsc", dump(cfg['environment'], Dumper=RoundTripDumper)), cfg['environment']) 39 | 40 | # shortcuts 41 | ob_dim = env.num_obs 42 | act_dim = env.num_acts 43 | 44 | 45 | # Training 46 | n_steps = math.floor(cfg['environment']['max_time'] / cfg['environment']['control_dt']) 47 | 48 | prop_enc_pth = '/'.join([base_dir, 'prop_encoder_' + runid + '.pt']) 49 | mlp_pth = '/'.join([base_dir, 'mlp_' + runid + '.pt']) 50 | 51 | env.load_scaling(base_dir, int(runid)) 52 | 53 | prop_loaded_encoder = torch.jit.load(prop_enc_pth) 54 | loaded_mlp = torch.jit.load(mlp_pth) 55 | 56 | 57 | 58 | print("Visualizing and evaluating the current policy") 59 | eplen = 0 60 | for update in range(1): 61 | env.reset() 62 | env.turn_on_visualization() 63 | 64 | eplen = 0 65 | env.set_itr_number(int(runid)) 66 | for step in range(50000): 67 | time.sleep(0.01) 68 | obs = env.observe(False) 69 | 70 | obs_torch = torch.from_numpy(obs).cpu() 71 | with torch.no_grad(): 72 | if step%2 == 0: 73 | latent_p = prop_loaded_encoder(obs_torch[:,:base_dims*t_steps]) 74 | action_ll = loaded_mlp(torch.cat([obs_torch[:,base_dims*t_steps : base_dims*(t_steps + 1)], 75 | latent_p], 1)) 76 | reward_ll, dones = env.step(action_ll.cpu().detach().numpy()) 77 | eplen+=1 78 | work = env.get_reward_info()[0][-1] 79 | if dones[0]: 80 | eplen = 0 81 | # you can add plots here 82 | 83 | env.turn_off_visualization() 84 | -------------------------------------------------------------------------------- /raisimGymTorch/env/RaisimGymEnv.hpp: -------------------------------------------------------------------------------- 1 | //----------------------------// 2 | // This file is part of RaiSim// 3 | // Copyright 2020, RaiSim Tech// 4 | //----------------------------// 5 | 6 | #ifndef SRC_RAISIMGYMENV_HPP 7 | #define SRC_RAISIMGYMENV_HPP 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "raisim/World.hpp" 14 | #include "raisim/RaisimServer.hpp" 15 | #include "Yaml.hpp" 16 | 17 | #define __RSG_MAKE_STR(x) #x 18 | #define _RSG_MAKE_STR(x) __RSG_MAKE_STR(x) 19 | #define RSG_MAKE_STR(x) _RSG_MAKE_STR(x) 20 | 21 | #define READ_YAML(a, b, c) RSFATAL_IF(!&c, "Node "<(); 22 | 23 | namespace raisim { 24 | 25 | using Dtype=float; 26 | using EigenRowMajorMat=Eigen::Matrix; 27 | using EigenVec=Eigen::Matrix; 28 | using EigenBoolVec=Eigen::Matrix; 29 | 30 | class RaisimGymEnv { 31 | 32 | public: 33 | explicit RaisimGymEnv (std::string resourceDir, const Yaml::Node& cfg) : resourceDir_(std::move(resourceDir)), cfg_(cfg) { 34 | world_ = std::make_unique(); 35 | } 36 | 37 | virtual ~RaisimGymEnv() { close(); }; 38 | 39 | /////// implement these methods ///////// 40 | virtual void init() = 0; 41 | virtual void reset(bool resample_target_speed) = 0; 42 | virtual void observe(Eigen::Ref ob) = 0; 43 | virtual float step(const Eigen::Ref& action) = 0; 44 | virtual bool isTerminalState(float& terminalReward) = 0; 45 | //////////////////////////////////////// 46 | 47 | /////// optional methods /////// 48 | virtual void curriculumUpdate() {}; 49 | virtual void close() { if(server_) server_->killServer(); }; 50 | virtual void setSeed(int seed) {}; 51 | //////////////////////////////// 52 | 53 | void setSimulationTimeStep(double dt) { simulation_dt_ = dt; world_->setTimeStep(dt); } 54 | void setItrNumber(int number) { itr_number = number;} 55 | void setControlTimeStep(double dt) { control_dt_ = dt; } 56 | int getObDim() { return obDim_; } 57 | int getActionDim() { return actionDim_; } 58 | double getControlTimeStep() { return control_dt_; } 59 | double getSimulationTimeStep() { return simulation_dt_; } 60 | raisim::World* getWorld() { return world_.get(); } 61 | void turnOffVisualization() { server_->hibernate(); } 62 | void turnOnVisualization() { server_->wakeup(); } 63 | void startRecordingVideo(const std::string& videoName ) { server_->startRecordingVideo(videoName); } 64 | void stopRecordingVideo() { server_->stopRecordingVideo(); } 65 | 66 | protected: 67 | std::unique_ptr world_; 68 | double simulation_dt_ = 0.001; 69 | double control_dt_ = 0.01; 70 | std::string resourceDir_; 71 | Yaml::Node cfg_; 72 | int obDim_=0, actionDim_=0; 73 | int itr_number = 0; 74 | std::unique_ptr server_; 75 | }; 76 | 77 | } 78 | 79 | #endif //SRC_RAISIMGYMENV_HPP 80 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import subprocess 4 | import platform 5 | 6 | from setuptools import Extension, find_packages 7 | from setuptools.command.build_ext import build_ext 8 | from distutils.core import setup 9 | 10 | __CMAKE_PREFIX_PATH__ = None 11 | __DEBUG__ = False 12 | 13 | if "--CMAKE_PREFIX_PATH" in sys.argv: 14 | index = sys.argv.index('--CMAKE_PREFIX_PATH') 15 | __CMAKE_PREFIX_PATH__ = sys.argv[index+1] 16 | sys.argv.remove("--CMAKE_PREFIX_PATH") 17 | sys.argv.remove(__CMAKE_PREFIX_PATH__) 18 | 19 | if "--Debug" in sys.argv: 20 | index = sys.argv.index('--Debug') 21 | sys.argv.remove("--Debug") 22 | __DEBUG__ = True 23 | 24 | class CMakeExtension(Extension): 25 | def __init__(self, name, sourcedir=''): 26 | Extension.__init__(self, name, sources=[]) 27 | self.sourcedir = os.path.abspath(sourcedir) 28 | 29 | class CMakeBuild(build_ext): 30 | def run(self): 31 | try: 32 | out = subprocess.check_output(['cmake', '--version']) 33 | except OSError: 34 | raise RuntimeError("CMake must be installed to build the following extensions: " + 35 | ", ".join(e.name for e in self.extensions)) 36 | 37 | for ext in self.extensions: 38 | self.build_extension(ext) 39 | 40 | def build_extension(self, ext): 41 | extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name))) 42 | cmake_args = ['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=' + extdir, 43 | '-DPYTHON_EXECUTABLE=' + sys.executable] 44 | 45 | if __CMAKE_PREFIX_PATH__ is not None: 46 | cmake_args.append('-DCMAKE_PREFIX_PATH=' + __CMAKE_PREFIX_PATH__) 47 | 48 | cfg = 'Debug' if __DEBUG__ else 'Release' 49 | build_args = ['--config', cfg] 50 | 51 | if platform.system() == "Windows": 52 | cmake_args += ['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{}={}'.format(cfg.upper(), extdir)] 53 | if sys.maxsize > 2**32: 54 | cmake_args += ['-A', 'x64'] 55 | build_args += ['--', '/m'] 56 | else: 57 | cmake_args += ['-DCMAKE_BUILD_TYPE=' + cfg] 58 | build_args += ['--', '-j4'] 59 | 60 | env = os.environ.copy() 61 | env['CXXFLAGS'] = '{} -DVERSION_INFO=\\"{}\\"'.format(env.get('CXXFLAGS', ''), 62 | self.distribution.get_version()) 63 | if not os.path.exists(self.build_temp): 64 | os.makedirs(self.build_temp) 65 | subprocess.check_call(['cmake', ext.sourcedir] + cmake_args, cwd=self.build_temp, env=env) 66 | subprocess.check_call(['cmake', '--build', '.'] + build_args, cwd=self.build_temp) 67 | 68 | setup( 69 | name='raisim_gym_torch', 70 | version='0.0.0', 71 | author='Jemin Hwangbo', 72 | license="proprietary", 73 | packages=find_packages(), 74 | author_email='jemin.hwangbo@gmail.com', 75 | description='gym for raisim using torch.', 76 | long_description='', 77 | ext_modules=[CMakeExtension('_raisim_gym')], 78 | install_requires=['ruamel.yaml', 'numpy', 'torch', 'tensorboard'], 79 | cmdclass=dict(build_ext=CMakeBuild), 80 | include_package_data=True, 81 | zip_safe=False, 82 | ) 83 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(_raisim_gym_torch) 3 | set(CMAKE_CXX_STANDARD 14) 4 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/raisimGymTorch/env/bin) 5 | SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/raisimGymTorch/env/bin) 6 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/raisimGymTorch/env/bin) 7 | 8 | 9 | MACRO(SUBDIRLIST result curdir) 10 | SET(dirlist "") 11 | LIST(APPEND dirlist rsg_a1_task) 12 | LIST(APPEND dirlist dagger_a1) 13 | SET(${result} ${dirlist}) 14 | ENDMACRO() 15 | 16 | #################### 17 | ### dependencies ### 18 | #################### 19 | set(Dependencies) 20 | 21 | add_subdirectory(../thirdParty/pybind11 pybind11) 22 | find_package(Eigen3 REQUIRED) 23 | find_package(OpenMP REQUIRED) 24 | 25 | if (UNIX AND NOT APPLE AND NOT ANDROID AND NOT WEBGL AND NOT WIN32) 26 | set(RAISIM_OS linux) 27 | list(APPEND CMAKE_PREFIX_PATH ../raisim/${RAISIM_OS}) 28 | elseif(APPLE) 29 | set(RAISIM_OS mac) 30 | list(APPEND CMAKE_PREFIX_PATH ../raisim/${RAISIM_OS}) 31 | list(APPEND Dependencies OpenMP::OpenMP_CXX) 32 | elseif(WIN32) 33 | set(RAISIM_OS win32) 34 | list(APPEND CMAKE_PREFIX_PATH ../raisim/${RAISIM_OS}/mt_debug) 35 | list(APPEND CMAKE_PREFIX_PATH ../thirdParty) 36 | list(APPEND Dependencies Ws2_32) 37 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY_RELEASE ${CMAKE_CURRENT_SOURCE_DIR}/raisimGymTorch/env/bin) 38 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE ${CMAKE_CURRENT_SOURCE_DIR}/raisimGymTorch/env/bin) 39 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE ${CMAKE_CURRENT_SOURCE_DIR}/raisimGymTorch/env/bin) 40 | endif() 41 | 42 | find_package(raisim CONFIG REQUIRED) 43 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 44 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 45 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 46 | 47 | ####################### 48 | ### src and linking ### 49 | ####################### 50 | SUBDIRLIST(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR}/raisimGymTorch/env/envs) 51 | set(RAISIMGYM_ENV_DIR ${CMAKE_CURRENT_SOURCE_DIR}/raisimGymTorch/env/envs) 52 | 53 | FOREACH(subdir ${SUBDIRS}) 54 | pybind11_add_module(${subdir} raisimGymTorch/env/raisim_gym.cpp raisimGymTorch/env/Yaml.cpp) 55 | target_link_libraries(${subdir} PRIVATE raisim::raisim ${Dependencies}) 56 | target_include_directories(${subdir} PUBLIC ${EIGEN3_INCLUDE_DIRS} ${RAISIMGYM_ENV_DIR}/${subdir}) 57 | target_compile_options(${subdir} PRIVATE -mtune=native -fPIC -O3 -g -mno-avx2) 58 | target_compile_definitions(${subdir} PRIVATE "-DRAISIMGYM_TORCH_ENV_NAME=${subdir}") 59 | 60 | if (CMAKE_BUILD_TYPE STREQUAL "DEBUG" OR CMAKE_BUILD_TYPE STREQUAL "Debug") 61 | message("[RAISIM_GYM] BUILDING THE DEBUG APP for ${subdir}") 62 | add_executable(${subdir}_debug_app raisimGymTorch/env/debug_app.cpp raisimGymTorch/env/Yaml.cpp) 63 | target_link_libraries(${subdir}_debug_app PRIVATE raisim::raisim) 64 | target_include_directories(${subdir}_debug_app PUBLIC raisimGymTorch/env/envs/${subdir} ${EIGEN3_INCLUDE_DIRS}) 65 | if(WIN32) 66 | target_link_libraries(${subdir}_debug_app PRIVATE Ws2_32) 67 | else() 68 | target_compile_options(${subdir}_debug_app PRIVATE -mtune=native -fPIC -g -O0 -mno-avx2) 69 | endif() 70 | endif() 71 | ENDFOREACH() 72 | -------------------------------------------------------------------------------- /raisimGymTorch/env/envs/rsg_a1_task/evaluate_policy.py: -------------------------------------------------------------------------------- 1 | from ruamel.yaml import YAML, dump, RoundTripDumper 2 | from raisimGymTorch.env.bin import rsg_a1_task 3 | from raisimGymTorch.env.RaisimGymVecEnv import RaisimGymVecEnv as VecEnv 4 | import os 5 | import time 6 | import numpy as np 7 | import torch 8 | import sys 9 | import pandas as pd 10 | 11 | VIZ = False 12 | # Evaluating Params 13 | num_episodes = 100 14 | 15 | base_dir = sys.argv[1] 16 | runid = sys.argv[2] 17 | 18 | # directories 19 | task_path = os.path.dirname(os.path.realpath(__file__)) 20 | home_path = task_path + "/../../../../.." 21 | 22 | # config 23 | cfg = YAML().load(open(sys.argv[1] + "/cfg.yaml", 'r')) 24 | # Multiple thread evaluation 25 | cfg['environment']['render'] = VIZ 26 | cfg['environment']['num_envs'] = 1 27 | cfg['environment']['num_threads'] = 1 28 | 29 | #cfg['environment']['randomize_friction'] = False 30 | #cfg['environment']['randomize_mass'] = False 31 | #cfg['environment']['randomize_motor_strength'] = False 32 | #cfg['environment']['randomize_gains'] = False 33 | #cfg['environment']['speedTest'] = False 34 | 35 | base_dims = cfg['environment']['baseDim'] 36 | n_futures = cfg['environment']['n_futures'] 37 | num_envs = cfg['environment']['num_envs'] 38 | cfg['environment']['test'] = VIZ 39 | cfg['environment']['eval'] = True 40 | 41 | # create environment from the configuration file 42 | env = VecEnv(rsg_a1_task.RaisimGymEnv(home_path + "/rsc", dump(cfg['environment'], Dumper=RoundTripDumper)), cfg['environment']) 43 | 44 | # shortcuts 45 | ob_dim = env.num_obs 46 | act_dim = env.num_acts 47 | 48 | # save the configuration and other files 49 | # saver = ConfigurationSaver(log_dir=home_path + "/raisimGymTorch/data/roughTerrain", 50 | # save_items=[task_path + "/cfg.yaml", task_path + "/Environment.hpp"]) 51 | 52 | 53 | env.load_scaling(base_dir, int(runid)) 54 | expert = torch.jit.load(os.path.join(base_dir, "policy_{}.pt".format(runid))) 55 | 56 | print("Visualizing and evaluating the current policy") 57 | env.reset() 58 | rng_seed = 100 59 | env.seed(rng_seed) 60 | torch.manual_seed(rng_seed) 61 | np.random.seed(rng_seed) 62 | if VIZ: 63 | env.turn_on_visualization() 64 | 65 | eplen = np.zeros(num_envs, dtype=int) 66 | latent_v = None 67 | env.set_itr_number(30000) 68 | ep = 0 69 | k = 0 70 | 71 | metrics_name = ['num_steps', 'forward_r', 'distance', 'energy', 'smoothness', 'ground_impact'] 72 | metrics_list = [] 73 | 74 | # Forward Reward, Distance, Work, Smoothness, Ground Impact 75 | metric_idxs = [0, 15, 11, 3, 7] 76 | metrics = np.zeros((num_envs, len(metric_idxs))) 77 | 78 | while ep < num_episodes: 79 | if VIZ: 80 | time.sleep(0.01) 81 | obs = env.observe(False) 82 | obs_torch = torch.from_numpy(obs).cpu() 83 | with torch.no_grad(): 84 | action_ll = expert.forward(torch.from_numpy(obs).cpu()) 85 | reward_ll, dones = env.step(action_ll.cpu().detach().numpy()) 86 | 87 | eplen+=1 88 | metrics += np.array(env.get_reward_info())[:, metric_idxs] 89 | terminated = np.where(dones == 1)[0].tolist() 90 | k += 1 91 | for i in terminated: 92 | #print("Lenght is {}".format(eplen[i])) 93 | row = [int(eplen[i])] 94 | row.extend(metrics[i].tolist()) 95 | if ep < num_episodes: 96 | metrics_list.append(row) 97 | ep += 1 98 | if ep % (num_episodes // 10) == 0: 99 | print("Done {}\%".format(ep / num_episodes*100)) 100 | eplen[i] = 0 101 | metrics[i] *= 0 102 | k = 0 103 | if VIZ: 104 | env.turn_off_visualization() 105 | env.turn_on_visualization() 106 | time.sleep(0.5) 107 | # Save as csv 108 | path = os.path.join(sys.argv[1], "evaluation_results.csv") 109 | pd.DataFrame(np.stack(metrics_list)).to_csv(path, header=metrics_name, index=None) 110 | -------------------------------------------------------------------------------- /raisimGymTorch/env/envs/dagger_a1/evaluate_policy.py: -------------------------------------------------------------------------------- 1 | from ruamel.yaml import YAML, dump, RoundTripDumper 2 | from raisimGymTorch.env.bin import dagger_a1 3 | from raisimGymTorch.env.RaisimGymVecEnv import RaisimGymVecEnv as VecEnv 4 | import os 5 | import time 6 | import numpy as np 7 | import torch 8 | import sys 9 | import pandas as pd 10 | 11 | VIZ = False 12 | use_expert_gamma = False 13 | use_expert_z = False 14 | # Evaluating Params 15 | num_episodes = 100 16 | 17 | base_dir = sys.argv[1] 18 | runid = sys.argv[2] 19 | 20 | # directories 21 | task_path = os.path.dirname(os.path.realpath(__file__)) 22 | home_path = task_path + "/../../../../.." 23 | 24 | # config 25 | cfg = YAML().load(open(sys.argv[1] + "/cfg.yaml", 'r')) 26 | # Multiple thread evaluation is not repeatable 27 | cfg['environment']['render'] = VIZ 28 | cfg['environment']['num_envs'] = 1 29 | cfg['environment']['num_threads'] = 1 30 | 31 | #cfg['environment']['randomize_friction'] = False 32 | #cfg['environment']['randomize_mass'] = False 33 | #cfg['environment']['randomize_motor_strength'] = False 34 | #cfg['environment']['randomize_gains'] = False 35 | #cfg['environment']['speedTest'] = False 36 | 37 | base_dims = cfg['environment']['baseDim'] 38 | n_futures = cfg['environment']['n_futures'] 39 | num_envs = cfg['environment']['num_envs'] 40 | cfg['environment']['test'] = False 41 | cfg['environment']['eval'] = True 42 | t_steps = cfg['environment']['history_len'] 43 | 44 | # create environment from the configuration file 45 | env = VecEnv(dagger_a1.RaisimGymEnv(home_path + "/rsc", dump(cfg['environment'], Dumper=RoundTripDumper)), cfg['environment']) 46 | 47 | # shortcuts 48 | ob_dim = env.num_obs 49 | act_dim = env.num_acts 50 | 51 | 52 | prop_enc_pth = '/'.join([base_dir, 'prop_encoder_' + runid + '.pt']) 53 | geom_enc_pth = '/'.join([base_dir, 'geom_encoder_' + runid + '.pt']) 54 | mlp_pth = '/'.join([base_dir, 'mlp_' + runid + '.pt']) 55 | 56 | env.load_scaling(base_dir, int(runid)) 57 | 58 | prop_loaded_encoder = torch.jit.load(prop_enc_pth) 59 | loaded_mlp = torch.jit.load(mlp_pth) 60 | 61 | if use_expert_gamma or use_expert_z: 62 | expert = torch.jit.load(os.path.join(base_dir, "policy_34000.pt")) 63 | 64 | print("Visualizing and evaluating the current policy") 65 | env.reset() 66 | rng_seed = 100 67 | env.seed(rng_seed) 68 | torch.manual_seed(rng_seed) 69 | np.random.seed(rng_seed) 70 | if VIZ: 71 | env.turn_on_visualization() 72 | 73 | eplen = np.zeros(num_envs, dtype=int) 74 | latent_v = None 75 | env.set_itr_number(int(runid)) 76 | ep = 0 77 | k = 0 78 | latent_p = None 79 | 80 | metrics_name = ['num_steps', 'forward_r', 'distance', 'energy', 'smoothness', 'ground_impact'] 81 | metrics_list = [] 82 | 83 | # Forward Reward, Distance, Work, Smoothness, Ground Impact 84 | metric_idxs = [0, 15, 11, 3, 7] 85 | metrics = np.zeros((num_envs, len(metric_idxs))) 86 | 87 | while ep < num_episodes: 88 | if VIZ: 89 | time.sleep(0.01) 90 | obs = env.observe(False) 91 | obs_torch = torch.from_numpy(obs).cpu() 92 | with torch.no_grad(): 93 | if k%2 == 0: 94 | if (latent_p is None) or (not use_expert_gamma or not use_expert_z): 95 | latent_p = prop_loaded_encoder(obs_torch[:,:base_dims*t_steps]) 96 | if use_expert_gamma: 97 | expert_g = expert.geom_encoder(obs_torch[:,-5:-3]) 98 | expert_g_2 = expert.geom_encoder(obs_torch[:,-3:-1]) 99 | latent_p[:,-2] = expert_g[:,0] 100 | latent_p[:,-1] = expert_g_2[:,0] 101 | if use_expert_z: 102 | expert_p = expert.prop_encoder(obs_torch[:,-28:-5]) 103 | latent_p[:,:8] = expert_p 104 | action_ll = loaded_mlp(torch.cat([obs_torch[:,base_dims*t_steps : base_dims*(t_steps + 1)], 105 | latent_p], 1)) 106 | reward_ll, dones = env.step(action_ll.cpu().detach().numpy()) 107 | 108 | eplen+=1 109 | metrics += np.array(env.get_reward_info())[:, metric_idxs] 110 | terminated = np.where(dones == 1)[0].tolist() 111 | k += 1 112 | for i in terminated: 113 | row = [int(eplen[i])] 114 | row.extend(metrics[i].tolist()) 115 | if ep < num_episodes: 116 | metrics_list.append(row) 117 | ep += 1 118 | if ep % (num_episodes // 10) == 0: 119 | print("Done {}\%".format(ep / num_episodes*100)) 120 | eplen[i] = 0 121 | metrics[i] *= 0 122 | k = 0 123 | if VIZ: 124 | env.turn_off_visualization() 125 | env.turn_on_visualization() 126 | time.sleep(0.5) 127 | # Save as csv 128 | path = os.path.join(sys.argv[1], "evaluation_results.csv") 129 | pd.DataFrame(np.stack(metrics_list)).to_csv(path, header=metrics_name, index=None) 130 | -------------------------------------------------------------------------------- /raisimGymTorch/env/VectorizedEnvironment.hpp: -------------------------------------------------------------------------------- 1 | //----------------------------// 2 | // This file is part of RaiSim// 3 | // Copyright 2020, RaiSim Tech// 4 | //----------------------------// 5 | 6 | #ifndef SRC_RAISIMGYMVECENV_HPP 7 | #define SRC_RAISIMGYMVECENV_HPP 8 | 9 | #include "RaisimGymEnv.hpp" 10 | #include "omp.h" 11 | #include "Yaml.hpp" 12 | 13 | namespace raisim { 14 | 15 | template 16 | class VectorizedEnvironment { 17 | 18 | public: 19 | 20 | explicit VectorizedEnvironment(std::string resourceDir, std::string cfg) 21 | : resourceDir_(resourceDir) { 22 | Yaml::Parse(cfg_, cfg); 23 | raisim::World::setActivationKey(raisim::Path(resourceDir + "/activation.raisim").getString()); 24 | // if(&cfg_["render"]) 25 | render_ = cfg_["render"].template As(); 26 | } 27 | 28 | ~VectorizedEnvironment() { 29 | for (auto *ptr: environments_) 30 | delete ptr; 31 | } 32 | 33 | void init() { 34 | omp_set_num_threads(cfg_["num_threads"].template As()); 35 | num_envs_ = cfg_["num_envs"].template As(); 36 | 37 | for (int i = 0; i < num_envs_; i++) { 38 | environments_.push_back(new ChildEnvironment(resourceDir_, cfg_, render_ && i == 0, i)); 39 | environments_.back()->setSimulationTimeStep(cfg_["simulation_dt"].template As()); 40 | environments_.back()->setControlTimeStep(cfg_["control_dt"].template As()); 41 | } 42 | 43 | setSeed(0); 44 | 45 | for (int i = 0; i < num_envs_; i++) { 46 | // only the first environment is visualized 47 | environments_[i]->init(); 48 | environments_[i]->reset(true); 49 | } 50 | 51 | obDim_ = environments_[0]->getObDim(); 52 | actionDim_ = environments_[0]->getActionDim(); 53 | RSFATAL_IF(obDim_ == 0 || actionDim_ == 0, "Observation/Action dimension must be defined in the constructor of each environment!") 54 | } 55 | 56 | // resets all environments and returns observation 57 | void reset() { 58 | for (auto env: environments_) 59 | env->reset(true); 60 | } 61 | 62 | void observe(Eigen::Ref &ob) { 63 | for (int i = 0; i < num_envs_; i++) 64 | environments_[i]->observe(ob.row(i)); 65 | } 66 | 67 | void step(Eigen::Ref &action, 68 | Eigen::Ref &reward, 69 | Eigen::Ref &done) { 70 | #pragma omp parallel for schedule(dynamic) 71 | for (int i = 0; i < num_envs_; i++) 72 | perAgentStep(i, action, reward, done); 73 | } 74 | 75 | void getDis(Eigen::Ref &dis){ 76 | #pragma omp parallel for schedule(dynamic) 77 | for (int i = 0; i < num_envs_; i++){ 78 | Eigen::Vector4f dis_vec; 79 | environments_[i]->getDis(dis_vec); 80 | dis.row(i) = dis_vec; 81 | } 82 | } 83 | 84 | void getRewardInfo(Eigen::Ref &rewardInfo){ 85 | #pragma omp parallel for schedule(dynamic) 86 | for (int i = 0; i < num_envs_; i++){ 87 | Eigen::Matrix r_vec; 88 | environments_[i]->getRewardInfo(r_vec); 89 | rewardInfo.row(i) = r_vec; 90 | } 91 | } 92 | 93 | void turnOnVisualization() { if(render_) environments_[0]->turnOnVisualization(); } 94 | void turnOffVisualization() { if(render_) environments_[0]->turnOffVisualization(); } 95 | void startRecordingVideo(const std::string& videoName) { if(render_) environments_[0]->startRecordingVideo(videoName); } 96 | void stopRecordingVideo() { if(render_) environments_[0]->stopRecordingVideo(); } 97 | 98 | void setSeed(int seed) { 99 | int seed_inc = seed; 100 | for (auto *env: environments_) 101 | env->setSeed(seed_inc++); 102 | } 103 | 104 | void setItrNumber(int number) { 105 | for (auto *env: environments_) 106 | env->setItrNumber(number); 107 | } 108 | 109 | void close() { 110 | for (auto *env: environments_) 111 | env->close(); 112 | } 113 | 114 | void isTerminalState(Eigen::Ref& terminalState) { 115 | for (int i = 0; i < num_envs_; i++) { 116 | float terminalReward; 117 | terminalState[i] = environments_[i]->isTerminalState(terminalReward); 118 | } 119 | } 120 | 121 | void setSimulationTimeStep(double dt) { 122 | for (auto *env: environments_) 123 | env->setSimulationTimeStep(dt); 124 | } 125 | 126 | void setControlTimeStep(double dt) { 127 | for (auto *env: environments_) 128 | env->setControlTimeStep(dt); 129 | } 130 | 131 | int getObDim() { return obDim_; } 132 | int getActionDim() { return actionDim_; } 133 | int getNumOfEnvs() { return num_envs_; } 134 | 135 | ////// optional methods ////// 136 | void curriculumUpdate() { 137 | for (auto *env: environments_) 138 | env->curriculumUpdate(); 139 | }; 140 | 141 | private: 142 | 143 | inline void perAgentStep(int agentId, 144 | Eigen::Ref &action, 145 | Eigen::Ref &reward, 146 | Eigen::Ref &done) { 147 | reward[agentId] = environments_[agentId]->step(action.row(agentId)); 148 | 149 | float terminalReward = 0; 150 | done[agentId] = environments_[agentId]->isTerminalState(terminalReward); 151 | 152 | if (done[agentId]) { 153 | environments_[agentId]->reset(false); 154 | reward[agentId] += terminalReward; 155 | } 156 | } 157 | 158 | std::vector environments_; 159 | 160 | int num_envs_ = 1; 161 | int obDim_ = 0, actionDim_ = 0; 162 | bool recordVideo_=false, render_=false; 163 | std::string resourceDir_; 164 | Yaml::Node cfg_; 165 | }; 166 | 167 | } 168 | 169 | #endif //SRC_RAISIMGYMVECENV_HPP 170 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Cross-Modal Supervision (Policy Training Code using RMA) 2 | 3 | This repo builds on the code from ```RMA: Rapid Motor Adaptation for Legged Robots``` to train the locomotion policy associated with the paper ```Learning Visual Locomotion with Cross-Modal Supervision```. 4 | For more information, please check the [RMA project page](https://ashish-kmr.github.io/rma-legged-robots/) and [CMS project webpage](https://antonilo.github.io/vision_locomotion/). For using the policy on a real robot, please refer to [this repository](https://github.com/antonilo/vision_locomotion). 5 | 6 | 7 | #### Paper, Video, and Datasets 8 | 9 | If you use this code in an academic context, please cite the following two publications: 10 | 11 | Papers: 12 | [RMA: Rapid Motor Adaptation for Legged Robots](https://arxiv.org/abs/2107.04034), Narration: [Video](https://www.youtube.com/watch?v=qKF6dr_S-wQ) 13 | [Learning Visual Locomotion With Cross-Modal Supervision](https://antonilo.github.io/vision_locomotion/pdf/manuscript.pdf), Narration: [Video](https://youtu.be/d7I34YIdMdk) 14 | 15 | ``` 16 | @InProceedings{kumar2021rma, 17 | author={Kumar, Ashish and Fu, Zipeng and Pathak, Deepak and Malik, Jitendra}, 18 | title={{RMA: Rapid motor adaptation for legged robots}}, 19 | booktitle={Robotics: Science and Systems}, 20 | year={2021} 21 | } 22 | 23 | @inproceedings{loquercio2023learning, 24 | title={Learning visual locomotion with cross-modal supervision}, 25 | author={Loquercio, Antonio and Kumar, Ashish and Malik, Jitendra}, 26 | booktitle={2023 IEEE International Conference on Robotics and Automation (ICRA)}, 27 | pages={7295--7302}, 28 | year={2023}, 29 | organization={IEEE} 30 | } 31 | ``` 32 | 33 | ## Usage 34 | 35 | This code trains a policy using reinforcement learning to walk on complex terrains with minimal information. The code uses the Raisim simulator for training. Note that simulator is CPU-based. 36 | 37 | ### Raisim Install 38 | 39 | Please follow the [installation guide](https://raisim.com/sections/Installation.html) of raisim. Note that we do not support the latest version of raisim. Please checkout the commit `f0bb440762c09a9cc93cf6ad3a7f8552c6a4f858` after cloning raisimLib. 40 | 41 | ### Training Environment Installation 42 | 43 | Run the following commands to install the training environments 44 | 45 | ``` 46 | cd raisimLib 47 | git clone git@github.com:antonilo/rl_locomotion.git 48 | rm -rf raisimGymTorch 49 | mv rl_locomotion raisimGymTorch 50 | cd raisimGymTorch 51 | # You might want to create a new conda environment if you did not do it already for the vision part 52 | conda create --name cms python=3.8 53 | conda activate cms 54 | conda install pytorch torchvision torchaudio pytorch-cuda=11.7 -c pytorch -c nvidia 55 | pip install opencv-python matplotlib pandas wandb 56 | 57 | # installation of the environments 58 | python setup.py develop 59 | ``` 60 | 61 | ### Training a policy with priviledged information 62 | 63 | You can use the following code to train a policy with access to priviledged information about the robot (e.g. mass, velocity, and motor strenght) and about the environment (e.g. the terrain geometry). The optimization will be guided by trajectories generated by [a previous policy](data/base_policy) we provide. You can control the strenght of the imitation by changing the parameter `RL_coeff` in [this file](./raisimGymTorch/env/envs/rsg_a1_task/runner.py). 64 | 65 | To start training, you can use the following commands: 66 | 67 | ``` 68 | cd raisimLib/raisimGymTorch/raisimGymTorch/env/envs/rsg_a1_task 69 | python runner.py --name random --gpu 1 --exptid 1 70 | ``` 71 | It will take approximately 4K iterations to train a good enough policy. If you want to make any changes to the training environment, feel free to edit [this file](./raisimGymTorch/env/envs/rsg_a1_task/Environment.hpp). Note that every time you make changes, you need to recompile the file by running this commands: 72 | 73 | ``` 74 | cd raisimLib/raisimGymTorch 75 | python setup.py develop 76 | ``` 77 | 78 | If you wish to continue a previous run, use the following commands: 79 | 80 | ``` 81 | cd raisimLib/raisimGymTorch/raisimGymTorch/env/envs/rsg_a1_task 82 | python runner.py --name random --gpu 1 --exptid 1 --loadid ITR_NBR --overwrite 83 | ``` 84 | 85 | ### Visualizing a policy 86 | 87 | You can use the following code to see if your policy training worked. First run the unity renderer: 88 | 89 | ``` 90 | cd raisimLib/raisimUnity/linux 91 | ./raisimUnity.x86_64 92 | ``` 93 | 94 | In a separate terminal, run the policy 95 | ``` 96 | conda activate cms 97 | cd raisimLib/raisimGymTorch/raisimGymTorch/env/envs/rsg_a1_task 98 | python viz_policy.py ../../../../data/rsg_a1_task/EXPT_ID POLICY_ID 99 | 100 | ``` 101 | You can now analysize the behaviour! 102 | 103 | ### Benchmark a policy 104 | 105 | If you want to know how the policy performs over a set of controlled experiments, use the following commands: 106 | ``` 107 | conda activate cms 108 | cd raisimLib/raisimGymTorch/raisimGymTorch/env/envs/rsg_a1_task 109 | python evaluate_policy.py ../../../../data/rsg_a1_task/EXPT_ID POLICY_ID 110 | 111 | ``` 112 | This will generate a json file in the experiment folder. You can visualize the results in a nice table using [this script](./eval_scripts/compute_results.py): 113 | 114 | ``` 115 | python compute_results.py ../data/rsg_a1_task/EXPT_ID/evaluation_results.csv 116 | 117 | ``` 118 | 119 | If you want to make any changes to the evaluation, feel free to edit [this file](./raisimGymTorch/env/envs/rsg_a1_task/Environment.hpp). Note that in the evaluation, the flag `Eval` is set to true. Remeber to recompile any time you edit the environment file! 120 | 121 | 122 | ### Distilling a policy with priviledged information into a blind policy 123 | 124 | A policy trained with priviledged information cannot be used on a physical robot. Therefore, we distill a priviledged policy using a slightly different version of [RMA](https://arxiv.org/abs/2107.04034) optimized for walking on complex terrains. 125 | 126 | To start training, you can use the following commands: 127 | 128 | ``` 129 | cd raisimLib/raisimGymTorch/raisimGymTorch/env/envs/dagger_a1 130 | python dagger.py --name cms_dagger --exptid 1 --loadpth ../../../../data/rsg_a1_task/EXPT_ID --loadid PRIV_POLICY_ID --gpu 1 131 | ``` 132 | It will take approximately 2K iterations to train a good enough policy. If you want to make any changes to the training environment, feel free to edit [this file](./raisimGymTorch/env/envs/dagger_a1/Environment.hpp). Note that every time you make changes, you need to recompile the environment (see above). 133 | 134 | ### Visualizing and evaluating a blind policy 135 | 136 | You can follow exactly the same steps as for the priviledged policy (but now running commands from [this folder](./raisimGymTorch/env/envs/dagger_a1/)) to visualize and benchmark a blind policy. 137 | 138 | ### Using a blind policy on a real robot 139 | 140 | If you want to use a policy you trained on a real robot, you should first move it in the [models folder](https://github.com/antonilo/vision_locomotion/tree/master/controller/models), change the path to the model in the [launch_file](https://github.com/antonilo/vision_locomotion/blob/master/controller/launch/cms_ros.launch#L7) and the policy id in the [parameter_file](https://github.com/antonilo/vision_locomotion/blob/master/controller/parameters/default.yaml#L2). 141 | -------------------------------------------------------------------------------- /raisimGymTorch/algo/ppo/storage.py: -------------------------------------------------------------------------------- 1 | import torch 2 | from torch.utils.data.sampler import BatchSampler, SubsetRandomSampler 3 | 4 | class ObsStorage: 5 | def __init__(self, num_envs, num_transitions_per_env, obs_shape, action_shape, device): 6 | self.device = device 7 | 8 | # Core 9 | self.obs = torch.zeros(num_transitions_per_env, num_envs, *obs_shape).to(self.device) 10 | self.expert = torch.zeros(num_transitions_per_env, num_envs, *action_shape).to(self.device) 11 | self.device = device 12 | 13 | self.num_envs = num_envs 14 | self.num_transitions_per_env = num_transitions_per_env 15 | self.step = 0 16 | 17 | def add_obs(self, obs, expert_action): 18 | if self.step >= self.num_transitions_per_env: 19 | raise AssertionError("Rollout buffer overflow") 20 | self.obs[self.step].copy_(torch.from_numpy(obs).to(self.device)) 21 | self.expert[self.step].copy_(expert_action) 22 | self.step += 1 23 | 24 | def clear(self): 25 | self.step = 0 26 | 27 | def mini_batch_generator_shuffle(self, num_mini_batches): 28 | batch_size = self.num_envs * self.num_transitions_per_env 29 | mini_batch_size = batch_size // num_mini_batches 30 | 31 | for indices in BatchSampler(SubsetRandomSampler(range(batch_size)), mini_batch_size, drop_last=True): 32 | obs_batch = self.obs.view(-1, *self.obs.size()[2:])[indices] 33 | expert_action_batch = self.expert.view(-1, *self.expert.size()[2:])[indices] 34 | yield obs_batch, expert_action_batch 35 | 36 | def mini_batch_generator_inorder(self, num_mini_batches): 37 | batch_size = self.num_envs * self.num_transitions_per_env 38 | mini_batch_size = batch_size // num_mini_batches 39 | 40 | for batch_id in range(num_mini_batches): 41 | yield self.obs.view(-1, *self.obs.size()[2:])[batch_id*mini_batch_size:(batch_id+1)*mini_batch_size], \ 42 | self.expert.view(-1, *self.expert.size()[2:])[batch_id*mini_batch_size:(batch_id+1)*mini_batch_size] 43 | 44 | class RolloutStorage: 45 | def __init__(self, num_envs, num_transitions_per_env, actor_obs_shape, critic_obs_shape, actions_shape, device): 46 | self.device = device 47 | 48 | # Core 49 | self.critic_obs = torch.zeros(num_transitions_per_env, num_envs, *actor_obs_shape).to(self.device) 50 | self.actor_obs = torch.zeros(num_transitions_per_env, num_envs, *critic_obs_shape).to(self.device) 51 | self.rewards = torch.zeros(num_transitions_per_env, num_envs, 1).to(self.device) 52 | self.actions = torch.zeros(num_transitions_per_env, num_envs, *actions_shape).to(self.device) 53 | self.dones = torch.zeros(num_transitions_per_env, num_envs, 1).byte().to(self.device) 54 | 55 | # For PPO 56 | self.actions_log_prob = torch.zeros(num_transitions_per_env, num_envs, 1).to(self.device) 57 | self.values = torch.zeros(num_transitions_per_env, num_envs, 1).to(self.device) 58 | self.returns = torch.zeros(num_transitions_per_env, num_envs, 1).to(self.device) 59 | self.advantages = torch.zeros(num_transitions_per_env, num_envs, 1).to(self.device) 60 | 61 | self.num_transitions_per_env = num_transitions_per_env 62 | self.num_envs = num_envs 63 | self.device = device 64 | 65 | self.step = 0 66 | 67 | def add_transitions(self, actor_obs, critic_obs, actions, rewards, dones, values, actions_log_prob): 68 | if self.step >= self.num_transitions_per_env: 69 | raise AssertionError("Rollout buffer overflow") 70 | self.critic_obs[self.step].copy_(torch.from_numpy(critic_obs).to(self.device)) 71 | self.actor_obs[self.step].copy_(torch.from_numpy(actor_obs).to(self.device)) 72 | self.actions[self.step].copy_(actions.to(self.device)) 73 | self.rewards[self.step].copy_(torch.from_numpy(rewards).view(-1, 1).to(self.device)) 74 | self.dones[self.step].copy_(torch.from_numpy(dones).view(-1, 1).to(self.device)) 75 | self.values[self.step].copy_(values.to(self.device)) 76 | self.actions_log_prob[self.step].copy_(actions_log_prob.view(-1, 1).to(self.device)) 77 | self.step += 1 78 | 79 | def clear(self): 80 | self.step = 0 81 | 82 | def compute_returns(self, last_values, gamma, lam): 83 | advantage = 0 84 | for step in reversed(range(self.num_transitions_per_env)): 85 | if step == self.num_transitions_per_env - 1: 86 | next_values = last_values 87 | # next_is_not_terminal = 1.0 - self.dones[step].float() 88 | else: 89 | next_values = self.values[step + 1] 90 | # next_is_not_terminal = 1.0 - self.dones[step+1].float() 91 | 92 | next_is_not_terminal = 1.0 - self.dones[step].float() 93 | delta = self.rewards[step] + next_is_not_terminal * gamma * next_values - self.values[step] 94 | advantage = delta + next_is_not_terminal * gamma * lam * advantage 95 | self.returns[step] = advantage + self.values[step] 96 | 97 | # Compute and normalize the advantages 98 | self.advantages = self.returns - self.values 99 | self.advantages = (self.advantages - self.advantages.mean()) / (self.advantages.std() + 1e-8) 100 | 101 | def mini_batch_generator_shuffle(self, num_mini_batches): 102 | batch_size = self.num_envs * self.num_transitions_per_env 103 | mini_batch_size = batch_size // num_mini_batches 104 | 105 | for indices in BatchSampler(SubsetRandomSampler(range(batch_size)), mini_batch_size, drop_last=True): 106 | actor_obs_batch = self.actor_obs.view(-1, *self.actor_obs.size()[2:])[indices] 107 | critic_obs_batch = self.critic_obs.view(-1, *self.critic_obs.size()[2:])[indices] 108 | actions_batch = self.actions.view(-1, self.actions.size(-1))[indices] 109 | values_batch = self.values.view(-1, 1)[indices] 110 | returns_batch = self.returns.view(-1, 1)[indices] 111 | old_actions_log_prob_batch = self.actions_log_prob.view(-1, 1)[indices] 112 | advantages_batch = self.advantages.view(-1, 1)[indices] 113 | yield actor_obs_batch, critic_obs_batch, actions_batch, values_batch, advantages_batch, returns_batch, old_actions_log_prob_batch 114 | 115 | def mini_batch_generator_inorder(self, num_mini_batches): 116 | batch_size = self.num_envs * self.num_transitions_per_env 117 | mini_batch_size = batch_size // num_mini_batches 118 | 119 | for batch_id in range(num_mini_batches): 120 | yield self.actor_obs.view(-1, *self.actor_obs.size()[2:])[batch_id*mini_batch_size:(batch_id+1)*mini_batch_size], \ 121 | self.critic_obs.view(-1, *self.critic_obs.size()[2:])[batch_id*mini_batch_size:(batch_id+1)*mini_batch_size], \ 122 | self.actions.view(-1, self.actions.size(-1))[batch_id*mini_batch_size:(batch_id+1)*mini_batch_size], \ 123 | self.values.view(-1, 1)[batch_id*mini_batch_size:(batch_id+1)*mini_batch_size], \ 124 | self.advantages.view(-1, 1)[batch_id*mini_batch_size:(batch_id+1)*mini_batch_size], \ 125 | self.returns.view(-1, 1)[batch_id*mini_batch_size:(batch_id+1)*mini_batch_size], \ 126 | self.actions_log_prob.view(-1, 1)[batch_id*mini_batch_size:(batch_id+1)*mini_batch_size] 127 | -------------------------------------------------------------------------------- /raisimGymTorch/algo/ppo/dagger.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.optim as optim 4 | import numpy as np 5 | from .storage import ObsStorage 6 | 7 | 8 | # computes and returns the latent from the expert 9 | class DaggerExpert(nn.Module): 10 | def __init__(self, loadpth, runid, total_obs_size, T, base_obs_size, nenvs, geomDim = 4, n_futures = 3): 11 | super(DaggerExpert, self).__init__() 12 | path = '/'.join([loadpth, 'policy_' + runid + '.pt']) 13 | self.policy = torch.jit.load(path) 14 | self.geomDim = geomDim 15 | self.n_futures = n_futures 16 | mean_pth = loadpth + "/mean" + runid + ".csv" 17 | var_pth = loadpth + "/var" + runid + ".csv" 18 | obs_mean = np.loadtxt(mean_pth, dtype=np.float32) 19 | obs_var = np.loadtxt(var_pth, dtype=np.float32) 20 | # cut it 21 | obs_mean = obs_mean[:,obs_mean.shape[1]//2:] 22 | obs_var = obs_var[:,obs_var.shape[1]//2:] 23 | self.mean = self.get_tiled_scales(obs_mean, nenvs, total_obs_size, base_obs_size, T) 24 | self.var = self.get_tiled_scales(obs_var, nenvs, total_obs_size, base_obs_size, T) 25 | self.tail_size = total_obs_size - (T + 1) * base_obs_size 26 | 27 | def get_tiled_scales(self, invec, nenvs, total_obs_size, base_obs_size, T): 28 | outvec = np.zeros([nenvs, total_obs_size], dtype = np.float32) 29 | outvec[:, :base_obs_size * T] = np.tile(invec[0, :base_obs_size], [1, T]) 30 | outvec[:, base_obs_size * T:] = invec[0] 31 | return outvec 32 | 33 | def forward(self, obs): 34 | obs = obs[:,-self.tail_size:] 35 | with torch.no_grad(): 36 | prop_latent = self.policy.prop_encoder(obs[:, :-self.geomDim*(self.n_futures+1)-1]) # since there is also ref at the end 37 | geom_latents = [] 38 | for i in reversed(range(self.n_futures+1)): 39 | start = -(i+1)*self.geomDim -1 40 | end = -i*self.geomDim -1 41 | if (end == 0): 42 | end = None 43 | geom_latent = self.policy.geom_encoder(obs[:,start:end]) 44 | geom_latents.append(geom_latent) 45 | geom_latents = torch.hstack(geom_latents) 46 | expert_latent = torch.cat((prop_latent, geom_latents), dim=1) 47 | return expert_latent 48 | 49 | class DaggerAgent: 50 | def __init__(self, expert_policy, 51 | prop_latent_encoder, student_mlp, 52 | T, base_obs_size, device, n_futures=3): 53 | expert_policy.to(device) 54 | prop_latent_encoder.to(device) 55 | #geom_latent_encoder.to(device) 56 | student_mlp.to(device) 57 | self.expert_policy = expert_policy 58 | self.prop_latent_encoder = prop_latent_encoder 59 | #self.geom_latent_encoder = geom_latent_encoder 60 | self.student_mlp = student_mlp 61 | self.base_obs_size = base_obs_size 62 | self.T = T 63 | self.device = device 64 | self.mean = expert_policy.mean 65 | self.var = expert_policy.var 66 | self.n_futures = n_futures 67 | self.itr = 0 68 | self.current_prob = 0 69 | # copy expert weights for mlp policy 70 | self.student_mlp.architecture.load_state_dict(self.expert_policy.policy.action_mlp.state_dict()) 71 | 72 | 73 | for net_i in [self.expert_policy.policy, self.student_mlp]: 74 | for param in net_i.parameters(): 75 | param.requires_grad = False 76 | 77 | def set_itr(self, itr): 78 | self.itr = itr 79 | if (itr+1) % 100 == 0: 80 | self.current_prob += 0.1 81 | print(f"Probability set to {self.current_prob}") 82 | 83 | def get_history_encoding(self, obs): 84 | hlen = self.base_obs_size * self.T 85 | raw_obs = obs[:, : hlen] 86 | # Hack to add velocity 87 | #velocity = obs[:, self.velocity_idx] -> Velocity thing is not robust 88 | #raw_obs[:, -3:] = velocity 89 | prop_latent = self.prop_latent_encoder(raw_obs) 90 | #geom_latent = self.geom_latent_encoder(raw_obs) 91 | return prop_latent 92 | 93 | def evaluate(self, obs): 94 | hlen = self.base_obs_size * self.T 95 | obdim = self.base_obs_size 96 | prop_latent = self.get_history_encoding(obs) 97 | #expert_latent = self.get_expert_latent(obs) 98 | #expert_future_geoms = expert_latent[:,prop_latent.shape[1]+geom_latent.shape[1]:] 99 | # assume that nothing changed 100 | #geom_latents = [] 101 | #for i in range(self.n_futures + 1): 102 | # geom_latents.append(geom_latent) 103 | #geom_latents = torch.hstack((geom_latent, expert_future_geoms)) 104 | #if np.random.random() < self.current_prob: 105 | # # student action 106 | output = torch.cat([obs[:, hlen : hlen + obdim], prop_latent], 1) 107 | #else: 108 | # # expert action 109 | # output = torch.cat([obs[:, hlen : hlen + obdim], expert_latent], 1) 110 | output = self.student_mlp.architecture(output) 111 | return output 112 | 113 | def get_expert_action(self, obs): 114 | hlen = self.base_obs_size * self.T 115 | obdim = self.base_obs_size 116 | expert_latent = self.get_expert_latent(obs) 117 | output = torch.cat([obs[:, hlen : hlen + obdim], expert_latent], 1) 118 | #else: 119 | # # expert action 120 | # output = torch.cat([obs[:, hlen : hlen + obdim], expert_latent], 1) 121 | output = self.student_mlp.architecture(output) 122 | return output 123 | 124 | def get_student_action(self, obs): 125 | return self.evaluate(obs) 126 | 127 | def get_expert_latent(self, obs): 128 | with torch.no_grad(): 129 | latent = self.expert_policy(obs).detach() 130 | return latent 131 | 132 | def save_deterministic_graph(self, fname_prop_encoder, 133 | fname_mlp, example_input, device='cpu'): 134 | hlen = self.base_obs_size * self.T 135 | 136 | prop_encoder_graph = torch.jit.trace(self.prop_latent_encoder.to(device), example_input[:, :hlen]) 137 | torch.jit.save(prop_encoder_graph, fname_prop_encoder) 138 | 139 | #geom_encoder_graph = torch.jit.trace(self.geom_latent_encoder.to(device), example_input[:, :hlen]) 140 | #torch.jit.save(geom_encoder_graph, fname_geom_encoder) 141 | 142 | mlp_graph = torch.jit.trace(self.student_mlp.architecture.to(device), example_input[:, hlen:]) 143 | torch.jit.save(mlp_graph, fname_mlp) 144 | 145 | self.prop_latent_encoder.to(self.device) 146 | #self.geom_latent_encoder.to(self.device) 147 | self.student_mlp.to(self.device) 148 | 149 | class DaggerTrainer: 150 | def __init__(self, 151 | actor, 152 | num_envs, 153 | num_transitions_per_env, 154 | obs_shape, latent_shape, 155 | num_learning_epochs=4, 156 | num_mini_batches=4, 157 | device=None, 158 | learning_rate=5e-4): 159 | 160 | self.actor = actor 161 | self.storage = ObsStorage(num_envs, num_transitions_per_env, [obs_shape], [latent_shape], device) 162 | self.optimizer = optim.Adam([*self.actor.prop_latent_encoder.parameters()], 163 | lr=learning_rate) 164 | self.scheduler = optim.lr_scheduler.StepLR(self.optimizer, step_size=200, gamma=0.1) 165 | self.device = device 166 | self.itr = 0 167 | 168 | # env parameters 169 | self.num_transitions_per_env = num_transitions_per_env 170 | self.num_envs = num_envs 171 | self.num_learning_epochs = num_learning_epochs 172 | self.num_mini_batches = num_mini_batches 173 | self.loss_fn = nn.MSELoss() 174 | 175 | def observe(self, obs): 176 | with torch.no_grad(): 177 | actions = self.actor.get_student_action(torch.from_numpy(obs).to(self.device)) 178 | #actions = self.actor.get_expert_action(torch.from_numpy(obs).to(self.device)) 179 | return actions.detach().cpu().numpy() 180 | 181 | def step(self, obs): 182 | expert_latent = self.actor.get_expert_latent(torch.from_numpy(obs).to(self.device)) 183 | self.storage.add_obs(obs, expert_latent) 184 | 185 | def update(self): 186 | # Learning step 187 | mse_loss = self._train_step() 188 | self.storage.clear() 189 | return mse_loss 190 | 191 | def _train_step(self): 192 | self.itr += 1 193 | self.actor.set_itr(self.itr) 194 | for epoch in range(self.num_learning_epochs): 195 | # return loss in the last epoch 196 | prop_mse = 0 197 | geom_mse = 0 198 | loss_counter = 0 199 | for obs_batch, expert_action_batch in self.storage.mini_batch_generator_inorder(self.num_mini_batches): 200 | 201 | predicted_prop_latent = self.actor.get_history_encoding(obs_batch) 202 | loss_prop = self.loss_fn(predicted_prop_latent[:,:8], expert_action_batch[:,:8]) 203 | loss_geom = self.loss_fn(predicted_prop_latent[:,8:], 204 | expert_action_batch[:,8:]) 205 | loss = loss_geom + loss_prop 206 | 207 | # Gradient step 208 | self.optimizer.zero_grad() 209 | loss.backward() 210 | self.optimizer.step() 211 | prop_mse += loss_prop.item() 212 | geom_mse += loss_geom.item() 213 | loss_counter += 1 214 | 215 | avg_prop_loss = prop_mse / loss_counter 216 | avg_geom_loss = geom_mse / loss_counter 217 | 218 | self.scheduler.step() 219 | return avg_prop_loss, avg_geom_loss 220 | -------------------------------------------------------------------------------- /raisimGymTorch/env/RaisimGymVecEnv.py: -------------------------------------------------------------------------------- 1 | # //----------------------------// 2 | # // This file is part of RaiSim// 3 | # // Copyright 2020, RaiSim Tech// 4 | # //----------------------------// 5 | 6 | import numpy as np 7 | 8 | 9 | class RaisimGymVecEnv: 10 | 11 | def __init__(self, impl, cfg, normalize_ob=True, seed=0, normalize_rew=True, clip_obs=10.): 12 | self.normalize_ob = normalize_ob 13 | self.normalize_rew = normalize_rew 14 | self.clip_obs = clip_obs 15 | self.wrapper = impl 16 | self.wrapper.init() 17 | self.num_obs = self.wrapper.getObDim() 18 | self.num_acts = self.wrapper.getActionDim() 19 | self._observation = np.zeros([self.num_envs, self.num_obs], dtype=np.float32) 20 | self.obs_rms = RunningMeanStd(shape=[self.num_envs, self.num_obs]) 21 | self._reward = np.zeros(self.num_envs, dtype=np.float32) 22 | self._done = np.zeros(self.num_envs, dtype=np.bool) 23 | self.rewards = [[] for _ in range(self.num_envs)] 24 | self.displacements = np.zeros([self.num_envs, 4], dtype=np.float32) 25 | self.reward_info = np.zeros([self.num_envs, 16], dtype=np.float32) 26 | 27 | def seed(self, seed=None): 28 | self.wrapper.setSeed(seed) 29 | 30 | def set_command(self, command): 31 | self.wrapper.setCommand(command) 32 | 33 | def turn_on_visualization(self): 34 | self.wrapper.turnOnVisualization() 35 | 36 | def turn_off_visualization(self): 37 | self.wrapper.turnOffVisualization() 38 | 39 | def start_video_recording(self, file_name): 40 | self.wrapper.startRecordingVideo(file_name) 41 | 42 | def stop_video_recording(self): 43 | self.wrapper.stopRecordingVideo() 44 | 45 | def step(self, action): 46 | self.wrapper.step(action, self._reward, self._done) 47 | return self._reward.copy(), self._done.copy() 48 | 49 | def load_scaling(self, dir_name, iteration, policy_type=None, num_g1=None): 50 | # policy_tupe 0 is the flat 51 | # policy type 1 is the combines 52 | # policy type 2 is the blind policy, from which we load encoders 53 | mean_file_name = dir_name + "/mean" + str(iteration) + ".csv" 54 | var_file_name = dir_name + "/var" + str(iteration) + ".csv" 55 | loaded_mean = np.loadtxt(mean_file_name, dtype=np.float32) 56 | loaded_var = np.loadtxt(var_file_name, dtype=np.float32) 57 | #if policy_type == 0: (for cvpr) 58 | # #self.obs_rms.mean[:,:loaded_mean.shape[1]] = loaded_mean 59 | # #self.obs_rms.var[:,:loaded_var.shape[1]] = loaded_var 60 | # self.obs_rms.mean[:,:loaded_mean.shape[1]] = loaded_mean 61 | # self.obs_rms.var[:,:loaded_var.shape[1]] = loaded_var 62 | if policy_type == 0: 63 | assert num_g1 is not None, "You should provide num_g1" 64 | processed_mean = np.zeros_like(self.obs_rms.mean[:,self.obs_rms.mean.shape[1]//2:]) 65 | processed_var = np.zeros_like(self.obs_rms.var[:,self.obs_rms.mean.shape[1]//2:]) 66 | blind_mean = loaded_mean[:,loaded_mean.shape[1]//2:] 67 | blind_var = loaded_var[:,loaded_var.shape[1]//2:] 68 | g1_mean = blind_mean[:,-5:-3] 69 | g1_var = loaded_var[:,-5:-3] 70 | for i in range(num_g1+1): 71 | start = -3 - 2*i 72 | end = -1 - 2*i 73 | processed_mean[:,start:end] = g1_mean 74 | processed_var[:,start:end] = g1_var 75 | # copy action + prop+part 76 | processed_mean[:,:start] = blind_mean[:,:-5] 77 | processed_var[:,:start] = blind_var[:,:-5] 78 | # do not normalize slope 79 | processed_mean[:,-1] = 0.0 80 | processed_var[:,-1] = 1.0 81 | self.obs_rms.mean[:,:self.obs_rms.mean.shape[1]//2] = processed_mean 82 | self.obs_rms.var[:,:self.obs_rms.mean.shape[1]//2] = processed_var 83 | elif policy_type == 1: 84 | self.obs_rms.mean[:,self.obs_rms.mean.shape[1]//2:] = loaded_mean[:,self.obs_rms.mean.shape[1]//2:] 85 | self.obs_rms.var[:,self.obs_rms.mean.shape[1]//2:] = loaded_var[:,self.obs_rms.mean.shape[1]//2:] 86 | elif policy_type == 2: 87 | assert num_g1 is not None, "You should provide num_g1" 88 | processed_mean = np.zeros_like(self.obs_rms.mean[:,self.obs_rms.mean.shape[1]//2:]) 89 | processed_var = np.zeros_like(self.obs_rms.var[:,self.obs_rms.mean.shape[1]//2:]) 90 | blind_mean = loaded_mean[:,loaded_mean.shape[1]//2:] 91 | blind_var = loaded_var[:,loaded_var.shape[1]//2:] 92 | g1_mean = blind_mean[:,-5:-3] 93 | g1_var = loaded_var[:,-5:-3] 94 | for i in range(num_g1+1): 95 | start = -3 - 2*i 96 | end = -1 - 2*i 97 | processed_mean[:,start:end] = g1_mean 98 | processed_var[:,start:end] = g1_var 99 | # copy action + prop+part 100 | processed_mean[:,:start] = blind_mean[:,:-5] 101 | processed_var[:,:start] = blind_var[:,:-5] 102 | # do not normalize slope 103 | processed_mean[:,-1] = 0.0 104 | processed_var[:,-1] = 1.0 105 | self.obs_rms.mean[:,self.obs_rms.mean.shape[1]//2:] = processed_mean 106 | self.obs_rms.var[:,self.obs_rms.mean.shape[1]//2:] = processed_var 107 | else: 108 | # All other cases including dagger and so 109 | self.obs_rms.mean = loaded_mean 110 | self.obs_rms.var = loaded_var 111 | # duplicate 112 | #self.obs_rms.mean = np.tile(self.obs_rms.mean, [1,2]) 113 | #self.obs_rms.var = np.tile(self.obs_rms.var, [1,2]) 114 | 115 | 116 | def save_scaling(self, dir_name, iteration): 117 | mean_file_name = dir_name + "/mean" + iteration + ".csv" 118 | var_file_name = dir_name + "/var" + iteration + ".csv" 119 | np.savetxt(mean_file_name, self.obs_rms.mean) 120 | np.savetxt(var_file_name, self.obs_rms.var) 121 | 122 | def observe(self, update_mean=True): 123 | self.wrapper.observe(self._observation) 124 | 125 | if self.normalize_ob: 126 | if update_mean: 127 | self.obs_rms.update(self._observation) 128 | 129 | return self._normalize_observation(self._observation) 130 | else: 131 | return self._observation.copy() 132 | 133 | def reset(self): 134 | self._reward = np.zeros(self.num_envs, dtype=np.float32) 135 | self.wrapper.reset() 136 | 137 | def _normalize_observation(self, obs): 138 | if self.normalize_ob: 139 | 140 | return np.clip((obs - self.obs_rms.mean) / np.sqrt(self.obs_rms.var + 1e-8), -self.clip_obs, 141 | self.clip_obs) 142 | else: 143 | return obs 144 | 145 | def get_dis(self): 146 | self.wrapper.getDis(self.displacements) 147 | return self.displacements.copy() 148 | 149 | def get_reward_info(self): 150 | self.wrapper.getRewardInfo(self.reward_info) 151 | return self.reward_info.copy() 152 | 153 | def reset_and_update_info(self): 154 | return self.reset(), self._update_epi_info() 155 | 156 | def _update_epi_info(self): 157 | info = [{} for _ in range(self.num_envs)] 158 | 159 | for i in range(self.num_envs): 160 | eprew = sum(self.rewards[i]) 161 | eplen = len(self.rewards[i]) 162 | epinfo = {"r": eprew, "l": eplen} 163 | info[i]['episode'] = epinfo 164 | self.rewards[i].clear() 165 | 166 | return info 167 | 168 | def close(self): 169 | self.wrapper.close() 170 | 171 | def curriculum_callback(self): 172 | self.wrapper.curriculumUpdate() 173 | 174 | def set_itr_number(self, itr_number): 175 | self.wrapper.setItrNumber(itr_number) 176 | 177 | @property 178 | def num_envs(self): 179 | return self.wrapper.getNumOfEnvs() 180 | 181 | @property 182 | def extra_info_names(self): 183 | return self._extraInfoNames 184 | 185 | 186 | class RunningMeanStd(object): 187 | def __init__(self, epsilon=1e-4, shape=()): 188 | """ 189 | calulates the running mean and std of a data stream 190 | https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Parallel_algorithm 191 | 192 | :param epsilon: (float) helps with arithmetic issues 193 | :param shape: (tuple) the shape of the data stream's output 194 | """ 195 | self.mean = np.zeros(shape, 'float32') 196 | self.var = np.ones(shape, 'float32') 197 | self.count = epsilon 198 | 199 | def update(self, arr): 200 | batch_mean = np.mean(arr[:,self.mean.shape[1]//2:], axis=0) 201 | batch_var = np.var(arr[:,self.var.shape[1]//2:], axis=0) 202 | batch_count = arr.shape[0] 203 | self.update_from_moments(batch_mean, batch_var, batch_count) 204 | 205 | def update_from_moments(self, batch_mean, batch_var, batch_count): 206 | half_mean = self.mean[:,self.mean.shape[1]//2:] 207 | half_var = self.var[:,self.var.shape[1]//2:] 208 | delta = batch_mean - half_mean 209 | tot_count = self.count + batch_count 210 | 211 | new_mean = half_mean + delta * batch_count / tot_count 212 | m_a = half_var * self.count 213 | m_b = batch_var * batch_count 214 | m_2 = m_a + m_b + np.square(delta) * (self.count * batch_count / (self.count + batch_count)) 215 | new_var = m_2 / (self.count + batch_count) 216 | 217 | new_count = batch_count + self.count 218 | 219 | self.mean[:,self.mean.shape[1]//2:] = new_mean 220 | self.var[:,self.var.shape[1]//2:] = new_var 221 | # account for the slope information 222 | self.mean[:,-1] = 0 223 | self.var[:,-1] = 1 224 | # duplicate future and present geometry 225 | self.mean[:,-5:-3] = self.mean[:,-3:-1] 226 | self.var[:,-5:-3] = self.var[:,-3:-1] 227 | self.count = new_count 228 | 229 | -------------------------------------------------------------------------------- /raisimGymTorch/algo/ppo/ppo.py: -------------------------------------------------------------------------------- 1 | from datetime import datetime 2 | import os 3 | import torch 4 | import torch.nn as nn 5 | import torch.optim as optim 6 | from torch.utils.tensorboard import SummaryWriter 7 | import numpy as np 8 | from .storage import RolloutStorage 9 | 10 | 11 | class PPO: 12 | def __init__(self, 13 | actor, 14 | critic, 15 | num_envs, 16 | num_transitions_per_env, 17 | num_learning_epochs, 18 | num_mini_batches, 19 | clip_param=0.2, 20 | gamma=0.998, 21 | lam=0.95, 22 | value_loss_coef=0.5, 23 | entropy_coef=0.0, 24 | learning_rate=5e-4, 25 | max_grad_norm=0.5, 26 | use_clipped_value_loss=True, 27 | log_dir='run', 28 | device='cpu', 29 | mini_batch_sampling='shuffle', 30 | log_intervals=10, 31 | flat_expert=None): 32 | 33 | # PPO components 34 | self.actor = actor 35 | self.critic = critic 36 | 37 | if actor.obs_shape[0] < 200: 38 | actor_obs_shape = actor.obs_shape[0]*2 39 | critic_obs_shape = critic.obs_shape[0]*2 40 | else: 41 | actor_obs_shape = actor.obs_shape[0] 42 | critic_obs_shape = critic.obs_shape[0] 43 | self.storage = RolloutStorage(num_envs, num_transitions_per_env, [critic_obs_shape], [actor_obs_shape], actor.action_shape, device) 44 | self.rl_coeff = 1 45 | 46 | if mini_batch_sampling == 'shuffle': 47 | self.batch_sampler = self.storage.mini_batch_generator_shuffle 48 | elif mini_batch_sampling == 'in_order': 49 | self.batch_sampler = self.storage.mini_batch_generator_inorder 50 | else: 51 | raise NameError(mini_batch_sampling + ' is not a valid sampling method. Use one of the followings: shuffle, order') 52 | 53 | self.optimizer = optim.Adam([*self.actor.parameters(), *self.critic.parameters()], lr=learning_rate) 54 | scheduler_lambda = lambda epoch: 0.9998 ** epoch 55 | self.scheduler = optim.lr_scheduler.LambdaLR(self.optimizer, lr_lambda=scheduler_lambda) 56 | self.device = device 57 | 58 | # env parameters 59 | self.num_transitions_per_env = num_transitions_per_env 60 | self.num_envs = num_envs 61 | 62 | # PPO parameters 63 | self.clip_param = clip_param 64 | self.num_learning_epochs = num_learning_epochs 65 | self.num_mini_batches = num_mini_batches 66 | self.value_loss_coef = value_loss_coef 67 | self.entropy_coef = entropy_coef 68 | self.gamma = gamma 69 | self.lam = lam 70 | self.max_grad_norm = max_grad_norm 71 | self.use_clipped_value_loss = use_clipped_value_loss 72 | 73 | # Log 74 | self.log_dir = os.path.join(log_dir, datetime.now().strftime('%b%d_%H-%M-%S')) 75 | self.writer = SummaryWriter(log_dir=self.log_dir, flush_secs=10) 76 | self.tot_timesteps = 0 77 | self.tot_time = 0 78 | self.ep_infos = [] 79 | self.log_intervals = log_intervals 80 | 81 | # temps 82 | self.actions = None 83 | self.actions_log_prob = None 84 | self.actor_obs = None 85 | 86 | # experts 87 | self.flat_expert = flat_expert 88 | self.imitation_loss = nn.MSELoss(reduction='none') 89 | 90 | 91 | def update_rl_coeff(self, coeffs): 92 | rl_coeff = np.clip(coeffs, 0, 1) 93 | self.rl_coeff = rl_coeff 94 | print("Setting RL coeffs to {}".format(self.rl_coeff)) 95 | 96 | def observe(self, actor_obs): 97 | self.actor_obs = actor_obs 98 | # the -1 is due to the addition of isSlope 99 | self.actions, self.actions_log_prob = self.actor.sample(torch.from_numpy(actor_obs).to(self.device)) 100 | # self.actions = np.clip(self.actions.numpy(), self.env.action_space.low, self.env.action_space.high) 101 | return self.actions.cpu().numpy() 102 | 103 | def step(self, value_obs, rews, dones, infos): 104 | value_obs = value_obs 105 | values = self.critic.predict(torch.from_numpy(value_obs).to(self.device)) 106 | self.storage.add_transitions(self.actor_obs, value_obs, self.actions, rews, dones, values, 107 | self.actions_log_prob) 108 | 109 | # Book keeping 110 | for info in infos: 111 | ep_info = info.get('episode') 112 | if ep_info is not None: 113 | self.ep_infos.append(ep_info) 114 | 115 | def update(self, actor_obs, value_obs, log_this_iteration, update): 116 | last_values = self.critic.predict(torch.from_numpy(value_obs).to(self.device)) 117 | 118 | # Learning step 119 | self.storage.compute_returns(last_values.to(self.device), self.gamma, self.lam) 120 | mean_value_loss, mean_surrogate_loss, infos = self._train_step() 121 | self.storage.clear() 122 | # stop = time.time() 123 | 124 | #if log_this_iteration: 125 | # self.log({**locals(), **infos, 'ep_infos': self.ep_infos, 'it': update}) 126 | 127 | self.ep_infos.clear() 128 | 129 | def log(self, variables, width=80, pad=28): 130 | self.tot_timesteps += self.num_transitions_per_env * self.num_envs 131 | 132 | ep_string = f'' 133 | for key in variables['ep_infos'][0]: 134 | value = np.mean([ep_info[key] for ep_info in variables['ep_infos']]) 135 | self.writer.add_scalar('Episode/' + key, value, variables['it']) 136 | ep_string += f"""{f'Mean episode {key}:':>{pad}} {value:.4f}\n""" 137 | mean_std = self.actor.distribution.log_std.exp().mean() 138 | 139 | self.writer.add_scalar('Loss/value_function', variables['mean_value_loss'], variables['it']) 140 | self.writer.add_scalar('Loss/surrogate', variables['mean_surrogate_loss'], variables['it']) 141 | self.writer.add_scalar('Policy/mean_noise_std', mean_std.item(), variables['it']) 142 | 143 | log_string = (f"""{'#' * width}\n""" 144 | f"""{'Value function loss:':>{pad}} {variables['mean_value_loss']:.4f}\n""" 145 | f"""{'Surrogate loss:':>{pad}} {variables['mean_surrogate_loss']:.4f}\n""" 146 | f"""{'Mean action noise std:':>{pad}} {mean_std.item():.2f}\n""") 147 | log_string += ep_string 148 | 149 | print(log_string) 150 | 151 | def _train_step(self): 152 | mean_value_loss = 0 153 | mean_surrogate_loss = 0 154 | for epoch in range(self.num_learning_epochs): 155 | for actor_obs_batch, critic_obs_batch, actions_batch, target_values_batch, advantages_batch, returns_batch, old_actions_log_prob_batch \ 156 | in self.storage.mini_batch_generator_inorder(self.num_mini_batches): 157 | 158 | (actions_log_prob_batch, entropy_batch), action_mean = self.actor.evaluate(actor_obs_batch, actions_batch) 159 | if self.flat_expert is not None: 160 | flat_actions = self.flat_expert.evaluate(actor_obs_batch) 161 | else: 162 | flat_actions = None 163 | isSlope = actor_obs_batch[:,-1] 164 | value_batch = self.critic.evaluate(critic_obs_batch) 165 | 166 | # Surrogate loss 167 | ratio = torch.exp(actions_log_prob_batch - torch.squeeze(old_actions_log_prob_batch)) 168 | surrogate = -torch.squeeze(advantages_batch) * ratio 169 | surrogate_clipped = -torch.squeeze(advantages_batch) * torch.clamp(ratio, 1.0 - self.clip_param, 170 | 1.0 + self.clip_param) 171 | surrogate_loss = torch.max(surrogate, surrogate_clipped) 172 | 173 | # Value function loss 174 | if self.use_clipped_value_loss: 175 | value_clipped = target_values_batch + (value_batch - target_values_batch).clamp(-self.clip_param, 176 | self.clip_param) 177 | value_losses = (value_batch - returns_batch).pow(2) 178 | value_losses_clipped = (value_clipped - returns_batch).pow(2) 179 | value_loss = torch.max(value_losses, value_losses_clipped) 180 | else: 181 | value_loss = (returns_batch - value_batch).pow(2) 182 | 183 | multiplicative_coeff_rl = self.rl_coeff*(1-isSlope) + isSlope 184 | 185 | rl_loss = (surrogate_loss + self.value_loss_coef * torch.squeeze(value_loss) - self.entropy_coef * entropy_batch) 186 | rl_loss = (multiplicative_coeff_rl * rl_loss).mean() 187 | if flat_actions is None: 188 | loss = rl_loss 189 | else: 190 | im_loss = (1-self.rl_coeff)*(1-isSlope)*torch.sum(self.imitation_loss(flat_actions, action_mean), dim=1) 191 | im_loss = im_loss.mean() 192 | loss = rl_loss + im_loss 193 | 194 | # Gradient step 195 | self.optimizer.zero_grad() 196 | loss.backward() 197 | if (self.actor.parameters()[-1].grad is None): 198 | print("No gradient for actor!") 199 | nn.utils.clip_grad_norm_([*self.actor.parameters(), *self.critic.parameters()], self.max_grad_norm) 200 | self.optimizer.step() 201 | 202 | mean_value_loss += value_loss.mean().item() 203 | mean_surrogate_loss += surrogate_loss.mean().item() 204 | 205 | num_updates = self.num_learning_epochs * self.num_mini_batches 206 | mean_value_loss /= num_updates 207 | mean_surrogate_loss /= num_updates 208 | 209 | return mean_value_loss, mean_surrogate_loss, locals() 210 | 211 | def update_scheduler(self): 212 | self.scheduler.step() 213 | 214 | -------------------------------------------------------------------------------- /raisimGymTorch/env/envs/dagger_a1/dagger.py: -------------------------------------------------------------------------------- 1 | from ruamel.yaml import YAML, dump, RoundTripDumper 2 | from raisimGymTorch.env.bin import dagger_a1 3 | from raisimGymTorch.env.RaisimGymVecEnv import RaisimGymVecEnv as VecEnv 4 | from raisimGymTorch.helper.raisim_gym_helper import ConfigurationSaver 5 | import os 6 | import math 7 | import time 8 | import raisimGymTorch.algo.ppo.module as ppo_module 9 | from raisimGymTorch.algo.ppo.dagger import DaggerExpert, DaggerAgent, DaggerTrainer 10 | import torch.nn as nn 11 | import numpy as np 12 | import torch 13 | import argparse 14 | try: 15 | import wandb 16 | except: 17 | wandb = None 18 | 19 | parser = argparse.ArgumentParser() 20 | parser.add_argument("--exptid", type = int, help='experiment id to prepend to the run') 21 | parser.add_argument("--overwrite", action = 'store_true') 22 | parser.add_argument("--debug", action = 'store_true') 23 | parser.add_argument("--loadpth", type = str, default = None) 24 | parser.add_argument("--loadid", type = str, default = None) 25 | parser.add_argument("--gpu", type = int, default = 0) 26 | parser.add_argument("--name", type = str) 27 | parser.add_argument("--ext_act", type = str, default='leakyRelu') 28 | args = parser.parse_args() 29 | 30 | 31 | # directories 32 | task_path = os.path.dirname(os.path.realpath(__file__)) 33 | home_path = task_path + "/../../../../.." 34 | 35 | 36 | # config 37 | cfg = YAML().load(open(args.loadpth + "/cfg.yaml", 'r')) 38 | with open("cfg.yaml", 'r') as f: 39 | dagger_cfg = YAML().load(f) 40 | # set seed 41 | rng_seed = cfg['seed'] 42 | torch.manual_seed(rng_seed) 43 | np.random.seed(rng_seed) 44 | 45 | t_steps = dagger_cfg['environment']['history_len'] 46 | base_dims = cfg['environment']['baseDim'] 47 | n_futures = int(dagger_cfg['environment']['n_futures']) 48 | cfg['environment']['n_futures'] = n_futures 49 | prop_latent_dim = 8 50 | geom_latent_dim = 1 51 | 52 | activation_fn_map = {'none': None, 'tanh': nn.Tanh, 'leakyRelu': nn.LeakyReLU} 53 | output_activation_fn = activation_fn_map[cfg['architecture']['activation']] 54 | small_init_flag = cfg['architecture']['small_init'] 55 | ext_activation_map = activation_fn_map[args.ext_act] 56 | 57 | if args.debug: 58 | cfg['environment']['num_envs'] = 1 59 | cfg['environment']['num_threads'] = 1 60 | device_type = 'cpu' 61 | else: 62 | device_type = 'cuda:{}'.format(args.gpu) 63 | 64 | cfg['environment']['num_threads'] = dagger_cfg['environment']['num_threads'] 65 | 66 | cfg['environment']['history_len'] = t_steps 67 | cfg['environment']['test'] = False 68 | cfg['environment']['eval'] = False 69 | 70 | # create environment from the configuration file 71 | env = VecEnv(dagger_a1.RaisimGymEnv(home_path + "/rsc", dump(cfg['environment'], Dumper=RoundTripDumper)), cfg['environment']) 72 | 73 | # shortcuts 74 | ob_dim = env.num_obs 75 | act_dim = env.num_acts 76 | 77 | 78 | priv_dim = ob_dim - base_dims * (t_steps + 1) 79 | 80 | # save a few logs about the run 81 | cfg['environment']['loadpth'] = args.loadpth 82 | cfg['environment']['loadid'] = args.loadid 83 | 84 | 85 | # save the configuration and other files 86 | saver = ConfigurationSaver(log_dir=home_path + "/raisimGymTorch/data/dagger_ckpt/" + '{:04d}'.format(args.exptid), 87 | save_items=[task_path + "/Environment.hpp", os.path.join(args.loadpth, f"policy_{args.loadid}.pt")], 88 | config = cfg, overwrite = args.overwrite) 89 | 90 | if wandb: 91 | wandb.init(project='command_loco', config=dict(cfg), name="dagger/" + args.name) 92 | wandb.save(home_path + '/raisimGymTorch/env/envs/rsg_a1_task/Environment.hpp') 93 | 94 | # Training 95 | n_steps = math.floor(cfg['environment']['max_time'] / cfg['environment']['control_dt']) 96 | total_steps = n_steps * env.num_envs 97 | 98 | avg_rewards = [] 99 | 100 | expert_policy = DaggerExpert(args.loadpth, args.loadid, ob_dim, t_steps, 101 | base_dims, env.obs_rms.mean.shape[0], 102 | geomDim=int(cfg['environment']['geomDim']), 103 | n_futures=n_futures) 104 | student_mlp = ppo_module.MLP(cfg['architecture']['policy_net'], 105 | ext_activation_map, 106 | base_dims + prop_latent_dim + (n_futures+1)*geom_latent_dim, 107 | act_dim, 108 | output_activation_fn, 109 | small_init_flag) 110 | prop_latent_encoder = ppo_module.StateHistoryEncoder(ext_activation_map, base_dims, t_steps, 111 | prop_latent_dim + (n_futures+1)*geom_latent_dim) 112 | 113 | actor = DaggerAgent(expert_policy, 114 | prop_latent_encoder, 115 | student_mlp, t_steps, base_dims, device_type, n_futures=n_futures) 116 | 117 | dagger = DaggerTrainer( 118 | actor=actor, 119 | num_envs=cfg['environment']['num_envs'], 120 | num_transitions_per_env=n_steps, 121 | obs_shape = ob_dim, 122 | latent_shape = prop_latent_dim + geom_latent_dim*(n_futures+1), 123 | num_learning_epochs=1, 124 | num_mini_batches=4, 125 | device=device_type, 126 | learning_rate=5e-3, 127 | ) 128 | 129 | env.obs_rms.mean = actor.mean 130 | env.obs_rms.var = actor.var 131 | 132 | penalty_scale = np.array([cfg['environment']['lateralVelRewardCoeff'], cfg['environment']['angularVelRewardCoeff'], cfg['environment']['deltaTorqueRewardCoeff'], cfg['environment']['actionRewardCoeff'], cfg['environment']['sidewaysRewardCoeff'], cfg['environment']['jointSpeedRewardCoeff'], cfg['environment']['deltaContactRewardCoeff'], cfg['environment']['deltaReleaseRewardCoeff'], cfg['environment']['footSlipRewardCoeff'], cfg['environment']['upwardRewardCoeff'], cfg['environment']['workRewardCoeff'], cfg['environment']['yAccRewardCoeff'], 1., 1., 1.]) 133 | 134 | 135 | env.set_itr_number(int(args.loadid)) 136 | 137 | for update in range(1201): 138 | start = time.time() 139 | env.reset() 140 | reward_ll_sum = 0 141 | forwardX_sum = 0 142 | penalty_sum = 0 143 | done_sum = 0 144 | average_dones = 0. 145 | 146 | if update % dagger_cfg['environment']['eval_every_n'] == 0: 147 | print("Saving the current policy") 148 | actor.save_deterministic_graph(saver.data_dir+"/prop_encoder_"+str(update)+'.pt', 149 | #saver.data_dir+"/geom_encoder_"+str(update)+'.pt', 150 | saver.data_dir+"/mlp_"+str(update)+'.pt', 151 | torch.rand(1, ob_dim - priv_dim + prop_latent_dim + (n_futures+1)*geom_latent_dim).cpu()) 152 | 153 | env.save_scaling(saver.data_dir, str(update)) 154 | 155 | # actual training 156 | for step in range(n_steps): 157 | obs = env.observe(update_mean=False) 158 | 159 | action = dagger.observe(obs) 160 | reward, dones = env.step(action) 161 | dagger.step(obs) 162 | unscaled_reward_info = env.get_reward_info() 163 | forwardX = unscaled_reward_info[:, 0] 164 | penalty = unscaled_reward_info[:, 1:] 165 | done_sum = done_sum + sum(dones) 166 | reward_ll_sum = reward_ll_sum + sum(reward) 167 | forwardX_sum += np.sum(forwardX) 168 | penalty_sum += np.sum(penalty, axis=0) 169 | 170 | env.curriculum_callback() 171 | 172 | # backward step 173 | prop_mse_loss, geom_mse_loss = dagger.update() 174 | 175 | end = time.time() 176 | forwardX = forwardX_sum / total_steps 177 | forwardXReward = forwardX_sum * cfg['environment']['forwardVelRewardCoeff'] / total_steps 178 | 179 | forwardY, forwardZ, deltaTorque, action, sideways, jointSpeed, deltaContact, deltaRelease, footSlip, upward, work, yAcc, torqueSquare, stepHeight, walkedDist = penalty_sum / total_steps 180 | forwardYReward, forwardZReward, deltaTorqueReward, actionReward, sidewaysReward, jointSpeedReward, deltaContactReward, deltaReleaseReward, footSlipReward, upwardReward, workReward, yAccReward, torq, stepHeight, walkedDist = scaled_penalty = penalty_sum * penalty_scale / total_steps 181 | 182 | average_ll_performance = reward_ll_sum / total_steps 183 | average_dones = done_sum / total_steps 184 | avg_rewards.append(average_ll_performance) 185 | 186 | 187 | if wandb: 188 | wandb.log({'forwardX': forwardX, 189 | 'forwardX_reward': forwardXReward, 190 | 'forwardY': forwardY, 191 | 'forwardY_reward': forwardYReward, 192 | 'forwardZ': forwardZ, 193 | 'forwardZ_reward': forwardZReward, 194 | 'deltaTorque': deltaTorque, 195 | 'deltaTorque_reward': deltaTorqueReward, 196 | 'action': action, 197 | 'stepHeight': stepHeight, 198 | 'action_reward': actionReward, 199 | 'sideways': sideways, 200 | 'sideways_reward': sidewaysReward, 201 | 'jointSpeed': jointSpeed, 202 | 'jointSpeed_reward': jointSpeedReward, 203 | 'deltaContact': deltaContact, 204 | 'deltaContact_reward': deltaContactReward, 205 | 'deltaRelease': deltaRelease, 206 | 'deltaRelease_reward': deltaReleaseReward, 207 | 'footSlip': footSlip, 208 | 'footSlip_reward': footSlipReward, 209 | 'upward': upward, 210 | 'upward_reward': upwardReward, 211 | 'work': work, 212 | 'work_reward': workReward, 213 | 'yAcc': yAcc, 214 | 'yAcc_reward': yAccReward, 215 | 'torqueSquare': torqueSquare, 216 | 'dones': average_dones, 217 | 'walkedDist': walkedDist, 218 | 'Latent Prop MSE': prop_mse_loss, 219 | 'Latent Geom MSE': geom_mse_loss}) 220 | 221 | print('----------------------------------------------------') 222 | print('{:>6}th iteration'.format(update)) 223 | print('{:<40} {:>6}'.format("average ll reward: ", '{:0.10f}'.format(average_ll_performance))) 224 | print('{:<40} {:>6}'.format("prop mse loss: ", '{:0.10f}'.format(prop_mse_loss))) 225 | print('{:<40} {:>6}'.format("geom mse loss: ", '{:0.10f}'.format(geom_mse_loss))) 226 | print('{:<40} {:>6}'.format("average forward reward: ", '{:0.10f}'.format(forwardXReward))) 227 | print('{:<40} {:>6}'.format("average penalty reward: ", ', '.join(['{:0.4f}'.format(r) for r in scaled_penalty]))) 228 | print('{:<40} {:>6}'.format("average walked dist: ", '{:0.10f}'.format(scaled_penalty[-1]))) 229 | print('{:<40} {:>6}'.format("dones: ", '{:0.6f}'.format(average_dones))) 230 | print('{:<40} {:>6}'.format("time elapsed in this iteration: ", '{:6.4f}'.format(end - start))) 231 | print('{:<40} {:>6}'.format("fps: ", '{:6.0f}'.format(total_steps / (end - start)))) 232 | print('----------------------------------------------------\n') 233 | -------------------------------------------------------------------------------- /raisimGymTorch/env/envs/rsg_a1_task/runner.py: -------------------------------------------------------------------------------- 1 | from statistics import geometric_mean 2 | from ruamel.yaml import YAML, dump, RoundTripDumper 3 | from raisimGymTorch.env.bin import rsg_a1_task 4 | from raisimGymTorch.env.RaisimGymVecEnv import RaisimGymVecEnv as VecEnv 5 | from raisimGymTorch.helper.raisim_gym_helper import ConfigurationSaver 6 | import os 7 | import math 8 | import time 9 | import raisimGymTorch.algo.ppo.module as ppo_module 10 | import raisimGymTorch.algo.ppo.ppo as PPO 11 | import torch.nn as nn 12 | import numpy as np 13 | import torch 14 | import datetime 15 | import argparse 16 | try: 17 | import wandb 18 | except: 19 | wandb = None 20 | 21 | parser = argparse.ArgumentParser() 22 | parser.add_argument("--exptid", type = int, help='experiment id to prepend to the run') 23 | parser.add_argument("--overwrite", action = 'store_true') 24 | parser.add_argument("--debug", action = 'store_true') 25 | parser.add_argument("--loadid", type = int, default = None) 26 | parser.add_argument("--gpu", type = int, default = 1) 27 | parser.add_argument("--name", type = str) 28 | args = parser.parse_args() 29 | 30 | # directories 31 | task_path = os.path.dirname(os.path.realpath(__file__)) 32 | home_path = task_path + "/../../../../.." 33 | 34 | # config 35 | cfg = YAML().load(open(task_path + "/cfg.yaml", 'r')) 36 | 37 | rng_seed = cfg['seed'] 38 | torch.manual_seed(rng_seed) 39 | np.random.seed(rng_seed) 40 | 41 | activation_fn_map = {'none': None, 'tanh': nn.Tanh} 42 | output_activation_fn = activation_fn_map[cfg['architecture']['activation']] 43 | small_init_flag = cfg['architecture']['small_init'] 44 | 45 | if args.debug: 46 | cfg['environment']['num_envs'] = 1 47 | cfg['environment']['num_threads'] = 1 48 | device_type = 'cpu' 49 | else: 50 | device_type = 'cuda:{}'.format(args.gpu) 51 | 52 | cfg['environment']['test'] = False 53 | cfg['environment']['speedTest'] = False 54 | 55 | baseDim = cfg['environment']['baseDim'] 56 | priv_info = cfg['environment']['privinfo'] 57 | use_fourier = 'fourier' in cfg['environment'] and cfg['environment']['fourier'] 58 | geomDim = int(cfg['environment']['geomDim'])*int(cfg['environment']['use_slope_dots']) 59 | n_futures = int(cfg['environment']['n_futures']) 60 | 61 | # create environment from the configuration file 62 | env = VecEnv(rsg_a1_task.RaisimGymEnv(home_path + "/rsc", dump(cfg['environment'], Dumper=RoundTripDumper)), cfg['environment']) 63 | 64 | # shortcuts 65 | ob_dim = env.num_obs 66 | act_dim = env.num_acts 67 | 68 | if use_fourier: 69 | privy_dim = ob_dim - baseDim 70 | encoder_dim = cfg['environment']['encoder_dim'] 71 | regular_fourier_dim = cfg['environment']['regular_fourier_dim'] 72 | privy_fourier_dim = cfg['environment']['privy_fourier_dim'] 73 | fourier_scale = cfg['environment']['fourier_scale'] 74 | fourier_trainable = cfg['environment']['fourier_trainable'] 75 | 76 | fourier_policy = cfg['environment']['fourier_policy'] 77 | fourier_value = cfg['environment']['fourier_value'] 78 | 79 | # save the configuration and other files 80 | saver = ConfigurationSaver(log_dir=home_path + "/raisimGymTorch/data/rsg_a1_task/" + '{:04d}'.format(args.exptid), 81 | save_items=[task_path + "/Environment.hpp", task_path + "/runner.py"], config = cfg, overwrite = args.overwrite) 82 | if wandb: 83 | wandb.init(project='command_loco', config=dict(cfg), name=args.name) 84 | wandb.save(home_path + '/raisimGymTorch/env/envs/rsg_a1_task/Environment.hpp') 85 | 86 | # Training 87 | n_steps = math.floor(cfg['environment']['max_time'] / cfg['environment']['control_dt']) 88 | total_steps = n_steps * env.num_envs 89 | 90 | if cfg['environment']['unnormalize_speed_vec']: 91 | raise NotImplementedError() 92 | 93 | speed_vec_start_idx = cfg['architecture']['speed_vec_start_idx'] 94 | speed_vec_end_idx = cfg['architecture']['speed_vec_end_idx'] 95 | layer_type = cfg['architecture']['layer_type'] 96 | freeze_encoder = cfg['architecture']['freeze_encoder'] 97 | 98 | avg_rewards = [] 99 | 100 | if use_fourier: 101 | raise Exception('not implemented') 102 | else: 103 | if priv_info: 104 | if layer_type == 'feedforward': 105 | init_var = 0.3 106 | module_type = ppo_module.MLPEncode_wrap 107 | actor = ppo_module.Actor(module_type(cfg['architecture']['policy_net'], 108 | nn.LeakyReLU, 109 | ob_dim//2, 110 | act_dim, 111 | output_activation_fn, 112 | small_init_flag, 113 | base_obdim = baseDim, 114 | geom_dim = geomDim, 115 | n_futures = n_futures), 116 | ppo_module.MultivariateGaussianDiagonalCovariance(act_dim, init_var), 117 | device_type) 118 | 119 | critic = ppo_module.Critic(module_type(cfg['architecture']['value_net'], 120 | nn.LeakyReLU, 121 | ob_dim//2, 122 | 1, 123 | base_obdim = baseDim, 124 | geom_dim = geomDim, 125 | n_futures = n_futures), 126 | device_type) 127 | else: 128 | raise NotImplementedError() 129 | 130 | else: 131 | raise NotImplementedError() 132 | 133 | # Steps + flat policy 134 | flat_policy_load_path = os.path.join(task_path,"../../../../data/base_policy/policy_22000.pt") 135 | env.load_scaling(os.path.join(task_path, "../../../../data/base_policy"), 136 | 22000, policy_type=0, num_g1=n_futures) 137 | loaded_graph_flat = torch.jit.load(flat_policy_load_path, map_location=torch.device(device_type)) 138 | flat_expert = ppo_module.Steps_Expert(loaded_graph_flat, device=device_type, baseDim=42, 139 | geomDim=2, n_futures=1, num_g1=n_futures) 140 | # Encoders loading from blind stairs policy 141 | checkpoint = torch.load(os.path.join(task_path,"../../../../data/base_policy/full_22000.pt")) 142 | blind_policy_state_dict = checkpoint['actor_architecture_state_dict'] 143 | own_state = actor.architecture.state_dict() 144 | for name, param in blind_policy_state_dict.items(): 145 | own_state[name].copy_(param) 146 | env.load_scaling(os.path.join(task_path, "../../../../data/base_policy"), 147 | 22000, policy_type=2, num_g1=n_futures) 148 | 149 | 150 | ppo = PPO.PPO(actor=actor, 151 | critic=critic, 152 | num_envs=cfg['environment']['num_envs'], 153 | num_transitions_per_env=n_steps, 154 | num_learning_epochs=4, 155 | gamma=0.997, 156 | lam=0.95, 157 | num_mini_batches=4, 158 | device=device_type, 159 | log_dir=saver.data_dir, 160 | mini_batch_sampling='in_order', 161 | learning_rate=5e-4, 162 | flat_expert=flat_expert 163 | ) 164 | 165 | if wandb: 166 | wandb.watch(actor.architecture.architecture, log_freq=100) 167 | wandb.watch(critic.architecture.architecture, log_freq=100) 168 | 169 | penalty_scale = np.array([cfg['environment']['lateralVelRewardCoeff'], cfg['environment']['angularVelRewardCoeff'], cfg['environment']['deltaTorqueRewardCoeff'], cfg['environment']['actionRewardCoeff'], cfg['environment']['sidewaysRewardCoeff'], cfg['environment']['jointSpeedRewardCoeff'], cfg['environment']['deltaContactRewardCoeff'], cfg['environment']['deltaReleaseRewardCoeff'], cfg['environment']['footSlipRewardCoeff'], cfg['environment']['upwardRewardCoeff'], cfg['environment']['workRewardCoeff'], cfg['environment']['yAccRewardCoeff'], 1., 1., 1.]) 170 | 171 | if args.loadid is not None: 172 | checkpoint = torch.load(saver.data_dir+"/full_"+str(args.loadid)+'.pt') 173 | actor.architecture.load_state_dict(checkpoint['actor_architecture_state_dict']) 174 | actor.distribution.load_state_dict(checkpoint['actor_distribution_state_dict']) 175 | critic.architecture.load_state_dict(checkpoint['critic_architecture_state_dict']) 176 | try: 177 | ppo.optimizer.load_state_dict(checkpoint['optimizer_state_dict']) 178 | except: 179 | print("Not loading ppo state") 180 | env.load_scaling(saver.data_dir, args.loadid, policy_type=1) 181 | 182 | if freeze_encoder: 183 | # do not update some networks 184 | print("Freezing the encoders!") 185 | for net_i in [actor.architecture.architecture.geom_encoder, 186 | actor.architecture.architecture.prop_encoder]: 187 | for param in net_i.parameters(): 188 | param.requires_grad = False 189 | 190 | if args.loadid is not None: 191 | env.set_itr_number(args.loadid) 192 | 193 | 194 | # This coefficient controls how much the policy is optimized with RL. Change to 1 for taking away demonstrations from a previous policy. 195 | rl_coeff = 0.3 196 | ppo.update_rl_coeff(rl_coeff) 197 | 198 | 199 | for update in range(500001) if args.loadid is None else range(args.loadid + 1, 500001): 200 | start = time.time() 201 | env.reset() 202 | reward_ll_sum = 0 203 | forwardX_sum = 0 204 | penalty_sum = 0 205 | done_sum = 0 206 | average_dones = 0. 207 | 208 | if update % cfg['environment']['eval_every_n'] == 0: 209 | print("Visualizing and evaluating the current policy") 210 | actor.save_deterministic_graph(saver.data_dir+"/policy_"+str(update)+'.pt', torch.rand(1, ob_dim).cpu()) 211 | if update % (1 * cfg['environment']['eval_every_n']) == 0: 212 | torch.save({ 213 | 'actor_architecture_state_dict': actor.architecture.state_dict(), 214 | 'actor_distribution_state_dict': actor.distribution.state_dict(), 215 | 'critic_architecture_state_dict': critic.architecture.state_dict(), 216 | 'optimizer_state_dict': ppo.optimizer.state_dict(), 217 | }, saver.data_dir+"/full_"+str(update)+'.pt') 218 | 219 | parameters = np.zeros([0], dtype=np.float32) 220 | for param in actor.deterministic_parameters(): 221 | parameters = np.concatenate([parameters, param.cpu().detach().numpy().flatten()], axis=0) 222 | np.savetxt(saver.data_dir+"/policy_"+str(update)+'.txt', parameters) 223 | loaded_graph = torch.jit.load(saver.data_dir+"/policy_"+str(update)+'.pt') 224 | 225 | env.reset() 226 | env.save_scaling(saver.data_dir, str(update)) 227 | 228 | # actual training 229 | for step in range(n_steps): 230 | obs = env.observe(not freeze_encoder) 231 | action = ppo.observe(obs) 232 | reward, dones = env.step(action) 233 | unscaled_reward_info = env.get_reward_info() 234 | forwardX = unscaled_reward_info[:, 0] 235 | penalty = unscaled_reward_info[:, 1:] 236 | ppo.step(value_obs=obs, rews=reward, dones=dones, infos=[]) 237 | done_sum = done_sum + sum(dones) 238 | reward_ll_sum = reward_ll_sum + sum(reward) 239 | forwardX_sum += np.sum(forwardX) 240 | penalty_sum += np.sum(penalty, axis=0) 241 | 242 | env.curriculum_callback() 243 | 244 | # take st step to get value obs 245 | obs = env.observe(not freeze_encoder) 246 | ppo.update(actor_obs=obs, 247 | value_obs=obs, 248 | log_this_iteration=update % 10 == 0, 249 | update=update) 250 | 251 | end = time.time() 252 | 253 | forwardX = forwardX_sum / total_steps 254 | forwardXReward = forwardX_sum * cfg['environment']['forwardVelRewardCoeff'] / total_steps 255 | 256 | forwardY, forwardZ, deltaTorque, action, sideways, jointSpeed, deltaContact, deltaRelease, footSlip, upward, work, yAcc, torqueSquare, stepHeight, walkedDist = penalty_sum / total_steps 257 | forwardYReward, forwardZReward, deltaTorqueReward, actionReward, sidewaysReward, jointSpeedReward, deltaContactReward, deltaReleaseReward, footSlipReward, upwardReward, workReward, yAccReward, torq, stepHeight, walkedDist = scaled_penalty = penalty_sum * penalty_scale / total_steps 258 | 259 | average_ll_performance = reward_ll_sum / total_steps 260 | average_dones = done_sum / total_steps 261 | avg_rewards.append(average_ll_performance) 262 | 263 | actor.distribution.enforce_minimum_std((torch.ones(12)*0.2).to(device_type)) 264 | if wandb: 265 | wandb.log({'forwardX': forwardX, 266 | 'forwardX_reward': forwardXReward, 267 | 'forwardY': forwardY, 268 | 'forwardY_reward': forwardYReward, 269 | 'forwardZ': forwardZ, 270 | 'forwardZ_reward': forwardZReward, 271 | 'deltaTorque': deltaTorque, 272 | 'deltaTorque_reward': deltaTorqueReward, 273 | 'action': action, 274 | 'stepHeight': stepHeight, 275 | 'action_reward': actionReward, 276 | 'sideways': sideways, 277 | 'sideways_reward': sidewaysReward, 278 | 'jointSpeed': jointSpeed, 279 | 'jointSpeed_reward': jointSpeedReward, 280 | 'deltaContact': deltaContact, 281 | 'deltaContact_reward': deltaContactReward, 282 | 'deltaRelease': deltaRelease, 283 | 'deltaRelease_reward': deltaReleaseReward, 284 | 'footSlip': footSlip, 285 | 'footSlip_reward': footSlipReward, 286 | 'upward': upward, 287 | 'upward_reward': upwardReward, 288 | 'work': work, 289 | 'work_reward': workReward, 290 | 'yAcc': yAcc, 291 | 'yAcc_reward': yAccReward, 292 | 'torqueSquare': torqueSquare, 293 | 'dones': average_dones, 294 | 'walkedDist': walkedDist}) 295 | 296 | print('----------------------------------------------------') 297 | print('{:>6}th iteration'.format(update)) 298 | print('{:<40} {:>6}'.format("average ll reward: ", '{:0.10f}'.format(average_ll_performance))) 299 | print('{:<40} {:>6}'.format("average forward reward: ", '{:0.10f}'.format(forwardXReward))) 300 | print('{:<40} {:>6}'.format("average penalty reward: ", ', '.join(['{:0.4f}'.format(r) for r in scaled_penalty]))) 301 | print('{:<40} {:>6}'.format("average walked dist: ", '{:0.10f}'.format(scaled_penalty[-1]))) 302 | print('{:<40} {:>6}'.format("dones: ", '{:0.6f}'.format(average_dones))) 303 | print('{:<40} {:>6}'.format("lr: ", '{:.4e}'.format(ppo.optimizer.param_groups[0]["lr"]))) 304 | print('{:<40} {:>6}'.format("time elapsed in this iteration: ", '{:6.4f}'.format(end - start))) 305 | print('{:<40} {:>6}'.format("fps: ", '{:6.0f}'.format(total_steps / (end - start)))) 306 | print('std: ') 307 | print(np.exp(actor.distribution.std.cpu().detach().numpy())) 308 | print('----------------------------------------------------\n') 309 | -------------------------------------------------------------------------------- /raisimGymTorch/env/Yaml.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MIT License 3 | * 4 | * Copyright(c) 2018 Jimmie Bergmann 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files(the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions : 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | * 24 | */ 25 | 26 | /* 27 | YAML documentation: 28 | http://yaml.org/spec/1.0/index.html 29 | https://www.codeproject.com/Articles/28720/YAML-Parser-in-C 30 | */ 31 | 32 | #pragma once 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | /** 42 | * @breif Namespace wrapping mini-yaml classes. 43 | * 44 | */ 45 | namespace Yaml 46 | { 47 | 48 | /** 49 | * @breif Forward declarations. 50 | * 51 | */ 52 | class Node; 53 | 54 | 55 | /** 56 | * @breif Helper classes and functions 57 | * 58 | */ 59 | namespace impl 60 | { 61 | 62 | /** 63 | * @breif Helper functionality, converting string to any data type. 64 | * Strings are left untouched. 65 | * 66 | */ 67 | template 68 | struct StringConverter 69 | { 70 | static T Get(const std::string & data) 71 | { 72 | T type; 73 | std::stringstream ss(data); 74 | ss >> type; 75 | return type; 76 | } 77 | 78 | static T Get(const std::string & data, const T & defaultValue) 79 | { 80 | T type; 81 | std::stringstream ss(data); 82 | ss >> type; 83 | 84 | if(ss.fail()) 85 | { 86 | return defaultValue; 87 | } 88 | 89 | return type; 90 | } 91 | }; 92 | template<> 93 | struct StringConverter 94 | { 95 | static std::string Get(const std::string & data) 96 | { 97 | return data; 98 | } 99 | 100 | static std::string Get(const std::string & data, const std::string & defaultValue) 101 | { 102 | if(data.size() == 0) 103 | { 104 | return defaultValue; 105 | } 106 | return data; 107 | } 108 | }; 109 | 110 | template<> 111 | struct StringConverter 112 | { 113 | static bool Get(const std::string & data) 114 | { 115 | std::string tmpData = data; 116 | std::transform(tmpData.begin(), tmpData.end(), tmpData.begin(), ::tolower); 117 | if(tmpData == "true" || tmpData == "yes" || tmpData == "1") 118 | { 119 | return true; 120 | } 121 | 122 | return false; 123 | } 124 | 125 | static bool Get(const std::string & data, const bool & defaultValue) 126 | { 127 | if(data.size() == 0) 128 | { 129 | return defaultValue; 130 | } 131 | 132 | return Get(data); 133 | } 134 | }; 135 | 136 | } 137 | 138 | 139 | /** 140 | * @breif Exception class. 141 | * 142 | */ 143 | class Exception : public std::runtime_error 144 | { 145 | 146 | public: 147 | 148 | /** 149 | * @breif Enumeration of exception types. 150 | * 151 | */ 152 | enum eType 153 | { 154 | InternalError, ///< Internal error. 155 | ParsingError, ///< Invalid parsing data. 156 | OperationError ///< User operation error. 157 | }; 158 | 159 | /** 160 | * @breif Constructor. 161 | * 162 | * @param message Exception message. 163 | * @param type Type of exception. 164 | * 165 | */ 166 | Exception(const std::string & message, const eType type); 167 | 168 | /** 169 | * @breif Get type of exception. 170 | * 171 | */ 172 | eType Type() const; 173 | 174 | /** 175 | * @breif Get message of exception. 176 | * 177 | */ 178 | const char * Message() const; 179 | 180 | private: 181 | 182 | eType m_Type; ///< Type of exception. 183 | 184 | }; 185 | 186 | 187 | /** 188 | * @breif Internal exception class. 189 | * 190 | * @see Exception 191 | * 192 | */ 193 | class InternalException : public Exception 194 | { 195 | 196 | public: 197 | 198 | /** 199 | * @breif Constructor. 200 | * 201 | * @param message Exception message. 202 | * 203 | */ 204 | InternalException(const std::string & message); 205 | 206 | }; 207 | 208 | 209 | /** 210 | * @breif Parsing exception class. 211 | * 212 | * @see Exception 213 | * 214 | */ 215 | class ParsingException : public Exception 216 | { 217 | 218 | public: 219 | 220 | /** 221 | * @breif Constructor. 222 | * 223 | * @param message Exception message. 224 | * 225 | */ 226 | ParsingException(const std::string & message); 227 | 228 | }; 229 | 230 | 231 | /** 232 | * @breif Operation exception class. 233 | * 234 | * @see Exception 235 | * 236 | */ 237 | class OperationException : public Exception 238 | { 239 | 240 | public: 241 | 242 | /** 243 | * @breif Constructor. 244 | * 245 | * @param message Exception message. 246 | * 247 | */ 248 | OperationException(const std::string & message); 249 | 250 | }; 251 | 252 | 253 | /** 254 | * @breif Iterator class. 255 | * 256 | */ 257 | class Iterator 258 | { 259 | 260 | public: 261 | 262 | friend class Node; 263 | 264 | /** 265 | * @breif Default constructor. 266 | * 267 | */ 268 | Iterator(); 269 | 270 | /** 271 | * @breif Copy constructor. 272 | * 273 | */ 274 | Iterator(const Iterator & it); 275 | 276 | /** 277 | * @breif Assignment operator. 278 | * 279 | */ 280 | Iterator & operator = (const Iterator & it); 281 | 282 | /** 283 | * @breif Destructor. 284 | * 285 | */ 286 | ~Iterator(); 287 | 288 | /** 289 | * @breif Get node of iterator. 290 | * First pair item is the key of map value, empty if type is sequence. 291 | * 292 | */ 293 | std::pair operator *(); 294 | 295 | /** 296 | * @breif Post-increment operator. 297 | * 298 | */ 299 | Iterator & operator ++ (int); 300 | 301 | /** 302 | * @breif Post-decrement operator. 303 | * 304 | */ 305 | Iterator & operator -- (int); 306 | 307 | /** 308 | * @breif Check if iterator is equal to other iterator. 309 | * 310 | */ 311 | bool operator == (const Iterator & it); 312 | 313 | /** 314 | * @breif Check if iterator is not equal to other iterator. 315 | * 316 | */ 317 | bool operator != (const Iterator & it); 318 | 319 | private: 320 | 321 | enum eType 322 | { 323 | None, 324 | SequenceType, 325 | MapType 326 | }; 327 | 328 | eType m_Type; ///< Type of iterator. 329 | void * m_pImp; ///< Implementation of iterator class. 330 | 331 | }; 332 | 333 | 334 | /** 335 | * @breif Constant iterator class. 336 | * 337 | */ 338 | class ConstIterator 339 | { 340 | 341 | public: 342 | 343 | friend class Node; 344 | 345 | /** 346 | * @breif Default constructor. 347 | * 348 | */ 349 | ConstIterator(); 350 | 351 | /** 352 | * @breif Copy constructor. 353 | * 354 | */ 355 | ConstIterator(const ConstIterator & it); 356 | 357 | /** 358 | * @breif Assignment operator. 359 | * 360 | */ 361 | ConstIterator & operator = (const ConstIterator & it); 362 | 363 | /** 364 | * @breif Destructor. 365 | * 366 | */ 367 | ~ConstIterator(); 368 | 369 | /** 370 | * @breif Get node of iterator. 371 | * First pair item is the key of map value, empty if type is sequence. 372 | * 373 | */ 374 | std::pair operator *(); 375 | 376 | /** 377 | * @breif Post-increment operator. 378 | * 379 | */ 380 | ConstIterator & operator ++ (int); 381 | 382 | /** 383 | * @breif Post-decrement operator. 384 | * 385 | */ 386 | ConstIterator & operator -- (int); 387 | 388 | /** 389 | * @breif Check if iterator is equal to other iterator. 390 | * 391 | */ 392 | bool operator == (const ConstIterator & it); 393 | 394 | /** 395 | * @breif Check if iterator is not equal to other iterator. 396 | * 397 | */ 398 | bool operator != (const ConstIterator & it); 399 | 400 | private: 401 | 402 | enum eType 403 | { 404 | None, 405 | SequenceType, 406 | MapType 407 | }; 408 | 409 | eType m_Type; ///< Type of iterator. 410 | void * m_pImp; ///< Implementation of constant iterator class. 411 | 412 | }; 413 | 414 | 415 | /** 416 | * @breif Node class. 417 | * 418 | */ 419 | class Node 420 | { 421 | 422 | public: 423 | 424 | friend class Iterator; 425 | 426 | /** 427 | * @breif Enumeration of node types. 428 | * 429 | */ 430 | enum eType 431 | { 432 | None, 433 | SequenceType, 434 | MapType, 435 | ScalarType 436 | }; 437 | 438 | /** 439 | * @breif Default constructor. 440 | * 441 | */ 442 | Node(); 443 | 444 | /** 445 | * @breif Copy constructor. 446 | * 447 | */ 448 | Node(const Node & node); 449 | 450 | /** 451 | * @breif Assignment constructors. 452 | * Converts node to scalar type if needed. 453 | * 454 | */ 455 | Node(const std::string & value); 456 | Node(const char * value); 457 | 458 | /** 459 | * @breif Destructor. 460 | * 461 | */ 462 | ~Node(); 463 | 464 | /** 465 | * @breif Functions for checking type of node. 466 | * 467 | */ 468 | eType Type() const; 469 | bool IsNone() const; 470 | bool IsSequence() const; 471 | bool IsMap() const; 472 | bool IsScalar() const; 473 | 474 | /** 475 | * @breif Completely clear node. 476 | * 477 | */ 478 | void Clear(); 479 | 480 | /** 481 | * @breif Get node as given template type. 482 | * 483 | */ 484 | template 485 | T As() const 486 | { 487 | return impl::StringConverter::Get(AsString()); 488 | } 489 | 490 | /** 491 | * @breif Get node as given template type. 492 | * 493 | */ 494 | template 495 | T As(const T & defaultValue) const 496 | { 497 | return impl::StringConverter::Get(AsString(), defaultValue); 498 | } 499 | 500 | /** 501 | * @breif Get size of node. 502 | * Nodes of type None or Scalar will return 0. 503 | * 504 | */ 505 | size_t Size() const; 506 | 507 | // Sequence operators 508 | 509 | /** 510 | * @breif Insert sequence item at given index. 511 | * Converts node to sequence type if needed. 512 | * Adding new item to end of sequence if index is larger than sequence size. 513 | * 514 | */ 515 | Node & Insert(const size_t index); 516 | 517 | /** 518 | * @breif Add new sequence index to back. 519 | * Converts node to sequence type if needed. 520 | * 521 | */ 522 | Node & PushFront(); 523 | 524 | /** 525 | * @breif Add new sequence index to front. 526 | * Converts node to sequence type if needed. 527 | * 528 | */ 529 | Node & PushBack(); 530 | 531 | /** 532 | * @breif Get sequence/map item. 533 | * Converts node to sequence/map type if needed. 534 | * 535 | * @param index Sequence index. Returns None type Node if index is unknown. 536 | * @param key Map key. Creates a new node if key is unknown. 537 | * 538 | */ 539 | Node & operator [] (const size_t index); 540 | const Node & operator [] (const size_t index) const; 541 | Node & operator [] (const std::string & key); 542 | const Node & operator [] (const std::string & key) const; 543 | 544 | /** 545 | * @breif Erase item. 546 | * No action if node is not a sequence or map. 547 | * 548 | */ 549 | void Erase(const size_t index); 550 | void Erase(const std::string & key); 551 | 552 | /** 553 | * @breif Assignment operators. 554 | * 555 | */ 556 | Node & operator = (const Node & node); 557 | Node & operator = (const std::string & value); 558 | Node & operator = (const char * value); 559 | 560 | /** 561 | * @breif Get start iterator. 562 | * 563 | */ 564 | Iterator Begin(); 565 | ConstIterator Begin() const; 566 | 567 | /** 568 | * @breif Get end iterator. 569 | * 570 | */ 571 | Iterator End(); 572 | ConstIterator End() const; 573 | 574 | 575 | private: 576 | 577 | /** 578 | * @breif Get as string. If type is scalar, else empty. 579 | * 580 | */ 581 | const std::string & AsString() const; 582 | 583 | void * m_pImp; ///< Implementation of node class. 584 | 585 | }; 586 | 587 | 588 | /** 589 | * @breif Parsing functions. 590 | * Population given root node with deserialized data. 591 | * 592 | * @param root Root node to populate. 593 | * @param filename Path of input file. 594 | * @param stream Input stream. 595 | * @param string String of input data. 596 | * @param buffer Char array of input data. 597 | * @param size Buffer size. 598 | * 599 | * @throw InternalException An internal error occurred. 600 | * @throw ParsingException Invalid input YAML data. 601 | * @throw OperationException If filename or buffer pointer is invalid. 602 | * 603 | */ 604 | void Parse(Node & root, const char * filename); 605 | void Parse(Node & root, std::iostream & stream); 606 | void Parse(Node & root, const std::string & string); 607 | void Parse(Node & root, const char * buffer, const size_t size); 608 | 609 | 610 | /** 611 | * @breif Serialization configuration structure, 612 | * describing output behavior. 613 | * 614 | */ 615 | struct SerializeConfig 616 | { 617 | 618 | /** 619 | * @breif Constructor. 620 | * 621 | * @param spaceIndentation Number of spaces per indentation. 622 | * @param scalarMaxLength Maximum length of scalars. Serialized as folder scalars if exceeded. 623 | * Ignored if equal to 0. 624 | * @param sequenceMapNewline Put maps on a new line if parent node is a sequence. 625 | * @param mapScalarNewline Put scalars on a new line if parent node is a map. 626 | * 627 | */ 628 | SerializeConfig(const size_t spaceIndentation = 2, 629 | const size_t scalarMaxLength = 64, 630 | const bool sequenceMapNewline = false, 631 | const bool mapScalarNewline = false); 632 | 633 | size_t SpaceIndentation; ///< Number of spaces per indentation. 634 | size_t ScalarMaxLength; ///< Maximum length of scalars. Serialized as folder scalars if exceeded. 635 | bool SequenceMapNewline; ///< Put maps on a new line if parent node is a sequence. 636 | bool MapScalarNewline; ///< Put scalars on a new line if parent node is a map. 637 | }; 638 | 639 | 640 | /** 641 | * @breif Serialization functions. 642 | * 643 | * @param root Root node to serialize. 644 | * @param filename Path of output file. 645 | * @param stream Output stream. 646 | * @param string String of output data. 647 | * @param config Serialization configurations. 648 | * 649 | * @throw InternalException An internal error occurred. 650 | * @throw OperationException If filename or buffer pointer is invalid. 651 | * If config is invalid. 652 | * 653 | */ 654 | void Serialize(const Node & root, const char * filename, const SerializeConfig & config = {2, 64, false, false}); 655 | void Serialize(const Node & root, std::iostream & stream, const SerializeConfig & config = {2, 64, false, false}); 656 | void Serialize(const Node & root, std::string & string, const SerializeConfig & config = {2, 64, false, false}); 657 | 658 | } 659 | -------------------------------------------------------------------------------- /raisimGymTorch/algo/ppo/module.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | import numpy as np 3 | import torch 4 | 5 | class Expert: 6 | def __init__(self, policy, device='cpu', baseDim=46): 7 | self.policy = policy 8 | self.policy.to(device) 9 | self.device = device 10 | self.baseDim = baseDim 11 | # 19 is size of priv info for the cvpr policy 12 | self.end_idx = baseDim + 19 #-self.geomDim*(self.n_futures+1) - 1 13 | 14 | def evaluate(self, obs): 15 | with torch.no_grad(): 16 | resized_obs = obs[:, :self.end_idx] 17 | latent = self.policy.info_encoder(resized_obs[:,self.baseDim:]) 18 | input_t = torch.cat((resized_obs[:,:self.baseDim], latent), dim=1) 19 | action = self.policy.action_mlp(input_t) 20 | return action 21 | 22 | class Steps_Expert: 23 | def __init__(self, policy, device='cpu', baseDim=42, geomDim=2, n_futures=1, num_g1=8): 24 | self.policy = policy 25 | self.policy.to(device) 26 | self.device = device 27 | self.baseDim = baseDim 28 | self.geom_dim = geomDim 29 | self.n_futures = n_futures 30 | # 23 is size of priv info for the cvpr policy 31 | self.privy_dim = 23 32 | self.end_prop_idx = baseDim + self.privy_dim 33 | self.g1_position = 0 34 | self.g1_slice = slice(self.end_prop_idx + self.g1_position * geomDim, 35 | self.end_prop_idx + (self.g1_position+1) * geomDim) 36 | self.g2_slice = slice(-geomDim-1,-1) 37 | 38 | def evaluate(self, obs): 39 | with torch.no_grad(): 40 | action = self.forward(obs) 41 | return action 42 | 43 | def forward(self, x): 44 | # get only the x related to the control policy 45 | x = x[:,:x.shape[1]//2] 46 | prop_latent = self.policy.prop_encoder(x[:,self.baseDim:self.end_prop_idx]) 47 | geom_latents = [] 48 | g1 = self.policy.geom_encoder(x[:,self.g1_slice]) 49 | g2 = self.policy.geom_encoder(x[:,self.g2_slice]) 50 | geom_latents = torch.hstack([g1,g2]) 51 | return self.policy.action_mlp(torch.cat([x[:,:self.baseDim], prop_latent, geom_latents], 1)) 52 | 53 | class Actor: 54 | def __init__(self, architecture, distribution, device='cpu'): 55 | super(Actor, self).__init__() 56 | 57 | self.architecture = architecture 58 | self.distribution = distribution 59 | try: 60 | self.architecture.to(device) 61 | except: 62 | print("If you're not in ARMA mode you have a problem") 63 | self.distribution.to(device) 64 | self.device = device 65 | 66 | def sample(self, obs): 67 | logits = self.architecture.architecture(obs) 68 | actions, log_prob = self.distribution.sample(logits) 69 | return actions.cpu().detach(), log_prob.cpu().detach() 70 | 71 | def evaluate(self, obs, actions): 72 | action_mean = self.architecture.architecture(obs) 73 | return self.distribution.evaluate(obs, action_mean, actions), action_mean 74 | 75 | def parameters(self): 76 | return [*self.architecture.parameters(), *self.distribution.parameters()] 77 | 78 | def noiseless_action(self, obs): 79 | return self.architecture.architecture(torch.from_numpy(obs).to(self.device)) 80 | 81 | def save_deterministic_graph(self, file_name, example_input, device='cpu'): 82 | transferred_graph = torch.jit.trace(self.architecture.architecture.to(device), example_input, check_trace=False) 83 | torch.jit.save(transferred_graph, file_name) 84 | self.architecture.architecture.to(self.device) 85 | 86 | def deterministic_parameters(self): 87 | return self.architecture.parameters() 88 | 89 | @property 90 | def obs_shape(self): 91 | return self.architecture.input_shape 92 | 93 | @property 94 | def action_shape(self): 95 | return self.architecture.output_shape 96 | 97 | class Critic: 98 | def __init__(self, architecture, device='cpu'): 99 | super(Critic, self).__init__() 100 | self.architecture = architecture 101 | self.architecture.to(device) 102 | 103 | def predict(self, obs): 104 | return self.architecture.architecture(obs).detach() 105 | 106 | def evaluate(self, obs): 107 | return self.architecture.architecture(obs) 108 | 109 | def parameters(self): 110 | return [*self.architecture.parameters()] 111 | 112 | @property 113 | def obs_shape(self): 114 | return self.architecture.input_shape 115 | 116 | class Flatten(nn.Module): 117 | def forward(self, x): 118 | return x.view(x.size(0), -1) 119 | 120 | def init(module, weight_init, bias_init, gain=1): 121 | weight_init(module.weight.data, gain=gain) 122 | bias_init(module.bias.data) 123 | return module 124 | 125 | class Action_MLP_Encode(nn.Module): 126 | def __init__(self, shape, actionvation_fn, input_size, output_size, output_activation_fn = None, small_init= False, base_obdim = 45, geom_dim = 0, 127 | n_futures = 3): 128 | super(Action_MLP_Encode, self).__init__() 129 | self.activation_fn = actionvation_fn 130 | self.output_activation_fn = output_activation_fn 131 | 132 | regular_obs_dim = base_obdim; 133 | self.regular_obs_dim = regular_obs_dim; 134 | self.geom_dim = geom_dim 135 | 136 | self.geom_latent_dim = 1 137 | self.prop_latent_dim = 8 138 | self.n_futures = n_futures 139 | 140 | # creating the action encoder 141 | modules = [nn.Linear(regular_obs_dim + self.prop_latent_dim + (self.n_futures + 1)*self.geom_latent_dim, shape[0]), self.activation_fn()] 142 | scale = [np.sqrt(2)] 143 | 144 | for idx in range(len(shape)-1): 145 | modules.append(nn.Linear(shape[idx], shape[idx+1])) 146 | modules.append(self.activation_fn()) 147 | scale.append(np.sqrt(2)) 148 | 149 | modules.append(nn.Linear(shape[-1], output_size)) 150 | action_output_layer = modules[-1] 151 | if self.output_activation_fn is not None: 152 | modules.append(self.output_activation_fn()) 153 | self.action_mlp = nn.Sequential(*modules) 154 | scale.append(np.sqrt(2)) 155 | 156 | self.init_weights(self.action_mlp, scale) 157 | 158 | self.input_shape = [input_size] 159 | self.output_shape = [output_size] 160 | 161 | @staticmethod 162 | def init_weights(sequential, scales): 163 | [torch.nn.init.orthogonal_(module.weight, gain=scales[idx]) for idx, module in 164 | enumerate(mod for mod in sequential if isinstance(mod, nn.Linear))] 165 | 166 | #for idx, module in enumerate(mod for mod in sequential if isinstance(mod, nn.Linear)): 167 | # module.weight.data *= 1e-6 168 | 169 | def forward(self, x): 170 | # get only the x related to the control policy 171 | geom_latent = x[:,-self.geom_dim:] 172 | prop_latent = x[:,-(self.geom_dim+self.prop_latent_dim):-self.geom_dim] 173 | return self.action_mlp(torch.cat([x[:,:self.regular_obs_dim], prop_latent, geom_latent], 1)) 174 | 175 | class Action_MLP_Encode_wrap(nn.Module): 176 | def __init__(self, shape, actionvation_fn, input_size, output_size, output_activation_fn = None, 177 | small_init= False, base_obdim = 45, geom_dim = 0, n_futures = 3): 178 | super(Action_MLP_Encode_wrap, self).__init__() 179 | self.architecture = Action_MLP_Encode(shape, actionvation_fn, input_size, output_size, output_activation_fn, small_init, base_obdim, geom_dim, n_futures) 180 | self.input_shape = self.architecture.input_shape 181 | self.output_shape = self.architecture.output_shape 182 | 183 | class MLPEncode(nn.Module): 184 | def __init__(self, shape, actionvation_fn, input_size, output_size, output_activation_fn = None, small_init= False, base_obdim = 45, geom_dim = 0, 185 | n_futures = 3): 186 | super(MLPEncode, self).__init__() 187 | self.activation_fn = actionvation_fn 188 | self.output_activation_fn = output_activation_fn 189 | 190 | regular_obs_dim = base_obdim; 191 | self.regular_obs_dim = regular_obs_dim; 192 | self.geom_dim = geom_dim 193 | 194 | ## Encoder Architecture 195 | prop_latent_dim = 8 196 | geom_latent_dim = 1 197 | self.n_futures = n_futures 198 | self.prop_latent_dim = prop_latent_dim 199 | self.geom_latent_dim = geom_latent_dim 200 | self.prop_encoder = nn.Sequential(*[ 201 | nn.Linear(input_size - (n_futures+1)*geom_dim - regular_obs_dim -1, 256), self.activation_fn(), 202 | nn.Linear(256, 128), self.activation_fn(), 203 | nn.Linear(128, prop_latent_dim), self.activation_fn(), 204 | ]) 205 | if self.geom_dim > 0: 206 | self.geom_encoder = nn.Sequential(*[ 207 | nn.Linear(geom_dim, 64), self.activation_fn(), 208 | nn.Linear(64, 16), self.activation_fn(), 209 | nn.Linear(16, geom_latent_dim), self.activation_fn(), 210 | ]) 211 | else: 212 | raise IOError("Not implemented geom_dim") 213 | scale_encoder = [np.sqrt(2), np.sqrt(2), np.sqrt(2)] 214 | 215 | # creating the action encoder 216 | modules = [nn.Linear(regular_obs_dim + prop_latent_dim + (self.n_futures + 1)*geom_latent_dim, shape[0]), self.activation_fn()] 217 | scale = [np.sqrt(2)] 218 | 219 | for idx in range(len(shape)-1): 220 | modules.append(nn.Linear(shape[idx], shape[idx+1])) 221 | modules.append(self.activation_fn()) 222 | scale.append(np.sqrt(2)) 223 | 224 | modules.append(nn.Linear(shape[-1], output_size)) 225 | action_output_layer = modules[-1] 226 | if self.output_activation_fn is not None: 227 | modules.append(self.output_activation_fn()) 228 | self.action_mlp = nn.Sequential(*modules) 229 | scale.append(np.sqrt(2)) 230 | 231 | self.init_weights(self.action_mlp, scale) 232 | self.init_weights(self.prop_encoder, scale_encoder) 233 | self.init_weights(self.geom_encoder, scale_encoder) 234 | if small_init: action_output_layer.weight.data *= 1e-6 235 | 236 | self.input_shape = [input_size] 237 | self.output_shape = [output_size] 238 | 239 | @staticmethod 240 | def init_weights(sequential, scales): 241 | [torch.nn.init.orthogonal_(module.weight, gain=scales[idx]) for idx, module in 242 | enumerate(mod for mod in sequential if isinstance(mod, nn.Linear))] 243 | 244 | #for idx, module in enumerate(mod for mod in sequential if isinstance(mod, nn.Linear)): 245 | # module.weight.data *= 1e-6 246 | 247 | def forward(self, x): 248 | # get only the x related to the control policy 249 | if x.shape[1] > 130: 250 | # Hacky way to detect where you are (dagger or not) 251 | # TODO: improve on this! 252 | x = x[:,x.shape[1]//2:] 253 | prop_latent = self.prop_encoder(x[:,self.regular_obs_dim:-self.geom_dim*(self.n_futures+1) -1]) 254 | geom_latents = [] 255 | for i in reversed(range(self.n_futures+1)): 256 | start = -(i+1)*self.geom_dim -1 257 | end = -i*self.geom_dim -1 258 | if (end == 0): 259 | end = None 260 | geom_latent = self.geom_encoder(x[:,start:end]) 261 | geom_latents.append(geom_latent) 262 | geom_latents = torch.hstack(geom_latents) 263 | return self.action_mlp(torch.cat([x[:,:self.regular_obs_dim], prop_latent, geom_latents], 1)) 264 | 265 | class MLPEncode_wrap(nn.Module): 266 | def __init__(self, shape, actionvation_fn, input_size, output_size, output_activation_fn = None, 267 | small_init= False, base_obdim = 45, geom_dim = 0, n_futures = 3): 268 | super(MLPEncode_wrap, self).__init__() 269 | self.architecture = MLPEncode(shape, actionvation_fn, input_size, output_size, output_activation_fn, small_init, base_obdim, geom_dim, n_futures) 270 | self.input_shape = self.architecture.input_shape 271 | self.output_shape = self.architecture.output_shape 272 | 273 | class StateHistoryEncoder(nn.Module): 274 | def __init__(self, activation_fn, input_size, tsteps, output_size): 275 | super(StateHistoryEncoder, self).__init__() 276 | self.activation_fn = activation_fn 277 | self.tsteps = tsteps 278 | self.input_shape = input_size*tsteps 279 | self.output_shape = output_size 280 | # self.encoder = nn.Sequential( 281 | # nn.Linear(input_size, 128), self.activation_fn(), 282 | # nn.Linear(128, 32), self.activation_fn() 283 | # ) 284 | 285 | if tsteps == 50: 286 | self.encoder = nn.Sequential( 287 | nn.Linear(input_size, 32), self.activation_fn() 288 | ) 289 | self.conv_layers = nn.Sequential( 290 | nn.Conv1d(in_channels = 32, out_channels = 32, kernel_size = 8, stride = 4), nn.LeakyReLU(), 291 | nn.Conv1d(in_channels = 32, out_channels = 32, kernel_size = 5, stride = 1), nn.LeakyReLU(), 292 | nn.Conv1d(in_channels = 32, out_channels = 32, kernel_size = 5, stride = 1), nn.LeakyReLU(), nn.Flatten()) 293 | self.linear_output = nn.Sequential( 294 | nn.Linear(32 * 3, output_size), self.activation_fn() 295 | ) 296 | elif tsteps == 10: 297 | self.encoder = nn.Sequential( 298 | nn.Linear(input_size, 32), self.activation_fn() 299 | ) 300 | self.conv_layers = nn.Sequential( 301 | nn.Conv1d(in_channels = 32, out_channels = 32, kernel_size = 4, stride = 2), nn.LeakyReLU(), 302 | nn.Conv1d(in_channels = 32, out_channels = 32, kernel_size = 2, stride = 1), nn.LeakyReLU(), 303 | nn.Flatten()) 304 | self.linear_output = nn.Sequential( 305 | nn.Linear(32 * 3, output_size), self.activation_fn() 306 | ) 307 | elif tsteps == 20: 308 | self.encoder = nn.Sequential( 309 | nn.Linear(input_size, 32), self.activation_fn() 310 | ) 311 | self.conv_layers = nn.Sequential( 312 | nn.Conv1d(in_channels = 32, out_channels = 32, kernel_size = 6, stride = 2), nn.LeakyReLU(), 313 | nn.Conv1d(in_channels = 32, out_channels = 32, kernel_size = 4, stride = 2), nn.LeakyReLU(), 314 | nn.Flatten()) 315 | self.linear_output = nn.Sequential( 316 | nn.Linear(32 * 3, output_size), self.activation_fn() 317 | ) 318 | else: 319 | raise NotImplementedError() 320 | 321 | 322 | 323 | def forward(self, obs): 324 | bs = obs.shape[0] 325 | T = self.tsteps 326 | projection = self.encoder(obs.reshape([bs * T, -1])) 327 | output = self.conv_layers(projection.reshape([bs, -1, T])) 328 | output = self.linear_output(output) 329 | return output 330 | 331 | class MLP(nn.Module): 332 | def __init__(self, shape, actionvation_fn, input_size, output_size, output_activation_fn = None, small_init= False, base_obdim = None): 333 | super(MLP, self).__init__() 334 | self.activation_fn = actionvation_fn 335 | self.output_activation_fn = output_activation_fn 336 | 337 | modules = [nn.Linear(input_size, shape[0]), self.activation_fn()] 338 | scale = [np.sqrt(2)] 339 | 340 | for idx in range(len(shape)-1): 341 | modules.append(nn.Linear(shape[idx], shape[idx+1])) 342 | modules.append(self.activation_fn()) 343 | scale.append(np.sqrt(2)) 344 | 345 | modules.append(nn.Linear(shape[-1], output_size)) 346 | action_output_layer = modules[-1] 347 | if self.output_activation_fn is not None: 348 | modules.append(self.output_activation_fn()) 349 | self.architecture = nn.Sequential(*modules) 350 | scale.append(np.sqrt(2)) 351 | 352 | self.init_weights(self.architecture, scale) 353 | if small_init: action_output_layer.weight.data *= 1e-6 354 | 355 | self.input_shape = [input_size] 356 | self.output_shape = [output_size] 357 | 358 | @staticmethod 359 | def init_weights(sequential, scales): 360 | [torch.nn.init.orthogonal_(module.weight, gain=scales[idx]) for idx, module in 361 | enumerate(mod for mod in sequential if isinstance(mod, nn.Linear))] 362 | 363 | #for idx, module in enumerate(mod for mod in sequential if isinstance(mod, nn.Linear)): 364 | # module.weight.data *= 1e-6 365 | 366 | class MultivariateGaussianDiagonalCovariance(nn.Module): 367 | def __init__(self, dim, init_std): 368 | super(MultivariateGaussianDiagonalCovariance, self).__init__() 369 | self.dim = dim 370 | self.std = nn.Parameter(init_std * torch.ones(dim)) 371 | self.distribution = None 372 | 373 | def sample(self, logits): 374 | self.distribution = Normal(logits, self.std.reshape(self.dim)) 375 | 376 | samples = self.distribution.sample() 377 | log_prob = self.distribution.log_prob(samples).sum(dim=1) 378 | 379 | return samples, log_prob 380 | 381 | def evaluate(self, inputs, logits, outputs): 382 | distribution = Normal(logits, self.std.reshape(self.dim)) 383 | 384 | actions_log_prob = distribution.log_prob(outputs).sum(dim=1) 385 | entropy = distribution.entropy().sum(dim=1) 386 | 387 | return actions_log_prob, entropy 388 | 389 | def entropy(self): 390 | return self.distribution.entropy() 391 | 392 | def enforce_minimum_std(self, min_std): 393 | current_std = self.std.detach() 394 | new_std = torch.max(current_std, min_std.detach()).detach() 395 | self.std.data = new_std 396 | 397 | class MultivariateGaussianDiagonalCovariance2(nn.Module): 398 | def __init__(self, dim, init_std): 399 | super(MultivariateGaussianDiagonalCovariance2, self).__init__() 400 | assert(dim == 12) 401 | self.dim = dim 402 | self.std_param = nn.Parameter(init_std * torch.ones(dim // 2)) 403 | self.distribution = None 404 | 405 | def sample(self, logits): 406 | self.std = torch.cat([self.std_param[:3], self.std_param[:3], self.std_param[3:], self.std_param[3:]], dim=0) 407 | self.distribution = Normal(logits, self.std.reshape(self.dim)) 408 | 409 | samples = self.distribution.sample() 410 | log_prob = self.distribution.log_prob(samples).sum(dim=1) 411 | 412 | return samples, log_prob 413 | 414 | def evaluate(self, inputs, logits, outputs): 415 | self.std = torch.cat([self.std_param[:3], self.std_param[:3], self.std_param[3:], self.std_param[3:]], dim=0) 416 | distribution = Normal(logits, self.std.reshape(self.dim)) 417 | 418 | actions_log_prob = distribution.log_prob(outputs).sum(dim=1) 419 | entropy = distribution.entropy().sum(dim=1) 420 | 421 | return actions_log_prob, entropy 422 | 423 | def entropy(self): 424 | return self.distribution.entropy() 425 | 426 | def enforce_minimum_std(self, min_std): 427 | current_std = self.std_param.detach() 428 | new_std = torch.max(current_std, min_std.detach()).detach() 429 | self.std_param.data = new_std 430 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | --------------------------------------------------------------------------------