diff --git a/.gitignore b/.gitignore index e4ed1c0b3b..69049dad7b 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,5 @@ dist *.egg-info *.so __pycache__ +build +saved diff --git a/flightlib/CMakeLists.txt b/flightlib/CMakeLists.txt index d1162ebb5d..ce1e36179c 100644 --- a/flightlib/CMakeLists.txt +++ b/flightlib/CMakeLists.txt @@ -27,6 +27,7 @@ if(EIGEN_FROM_SYSTTEM) find_package(Eigen3 3.3.4 QUIET) if(EIGEN3_FOUND) message(STATUS "Using system provided Eigen.") + message(${EIGEN3_INCLUDE_DIR}) else() message(STATUS "No sufficient Eigen version (3.3.4) found.") message(STATUS "Restoring to download Eigen sources.") @@ -35,9 +36,10 @@ if(EIGEN_FROM_SYSTTEM) elseif(EIGEN_ALTERNATIVE STREQUAL "") include(cmake/eigen.cmake) else() - set(EIGEN_INCLUDE_DIR ${EIGEN_ALTERNATIVE}) + set(EIGEN_INCLUDE_DIR ${cl}) endif() + # Including dependencies include(cmake/pybind11.cmake) include(cmake/yaml.cmake) @@ -198,7 +200,10 @@ if(NOT FLIGHTLIB_SOURCES) set(LIBRARY_NAME) else() # flightlib - add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES}) + add_library(${PROJECT_NAME} ${FLIGHTLIB_SOURCES}) + # target_include_directories(${PROJECT_NAME} PRIVATE + # ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include + # ) target_link_libraries(${PROJECT_NAME} PRIVATE ${OpenCV_LIBRARIES} yaml-cpp @@ -214,8 +219,9 @@ if(FLIGHTLIB_GYM_SOURCES) ${FLIGHTLIB_GYM_SOURCES}) target_include_directories(flightgym PRIVATE ${PROJECT_SOURCE_DIR}/externals/pybind11-src/include - ${PROJECT_SOURCE_DIR}/include) - target_link_libraries(flightgym PRIVATE ${LIBRARY_NAME}) + ${PROJECT_SOURCE_DIR}/include + ${EIGEN3_INCLUDE_DIR}) + target_link_libraries(flightgym PRIVATE ${LIBRARY_NAME} ) endif() if(ENABLE_BLAS AND BLAS_FOUND AND LAPACK_FOUND) diff --git a/flightlib/cmake/pybind11.cmake b/flightlib/cmake/pybind11.cmake index fc45a73040..7b3461f459 100644 --- a/flightlib/cmake/pybind11.cmake +++ b/flightlib/cmake/pybind11.cmake @@ -1,6 +1,9 @@ # Download and unpack pybind11 at configure time message(STATUS "Getting Pybind11...") +# set(PYBIND11_PYTHON_VERSION 3.6) +set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING}) + configure_file( cmake/pybind11_download.cmake ${PROJECT_SOURCE_DIR}/externals/pybind11/CMakeLists.txt) diff --git a/flightlib/cmake/pybind11_download.cmake b/flightlib/cmake/pybind11_download.cmake index a293f36db8..43672ae5da 100644 --- a/flightlib/cmake/pybind11_download.cmake +++ b/flightlib/cmake/pybind11_download.cmake @@ -5,7 +5,7 @@ project(pybind11-download) include(ExternalProject) ExternalProject_Add(pybind11 GIT_REPOSITORY https://github.com/pybind/pybind11.git - GIT_TAG master + GIT_TAG v2.4.3 SOURCE_DIR "${PROJECT_SOURCE_DIR}/externals/pybind11-src" BINARY_DIR "${PROJECT_SOURCE_DIR}/externals/pybind11-bin" CONFIGURE_COMMAND "" diff --git a/flightlib/configs/quadrotor_env.yaml b/flightlib/configs/quadrotor_env.yaml index 754161630c..66ba3fdfdc 100644 --- a/flightlib/configs/quadrotor_env.yaml +++ b/flightlib/configs/quadrotor_env.yaml @@ -1,3 +1,5 @@ +hello: 12 +render: no env: seed: 1 scene_id: 0 # 0 warehouse, 1 garage, 3 natureforest @@ -10,7 +12,7 @@ env: quadrotor: mass: 0.73 arm_length: 0.17 - rotor_drag_coeff: 0.016 + rotor_drag_coeff: 0.016 inertia: [0.007, 0.007, 0.012] camera: no diff --git a/flightlib/include/flightlib/bridges/unity_bridge.hpp b/flightlib/include/flightlib/bridges/unity_bridge.hpp index 18e14c5737..01014fb45a 100644 --- a/flightlib/include/flightlib/bridges/unity_bridge.hpp +++ b/flightlib/include/flightlib/bridges/unity_bridge.hpp @@ -8,7 +8,7 @@ #include #include -// Include ZMQ bindings for comms with Unity. +// Include ZMQ bindings for communications with Unity. #include // flightlib diff --git a/flightlib/include/flightlib/common/types.hpp b/flightlib/include/flightlib/common/types.hpp index 56f6f3152c..ea72996b05 100644 --- a/flightlib/include/flightlib/common/types.hpp +++ b/flightlib/include/flightlib/common/types.hpp @@ -7,7 +7,7 @@ namespace flightlib { // ------------ General Stuff------------- // Define the scalar type used. -using Scalar = double; +using Scalar = float; // Define frame id for unity using FrameID = uint64_t; @@ -27,7 +27,7 @@ template using Matrix = Eigen::Matrix; // Using shorthand for `Matrix` with scalar type. -template +template using MatrixRowMajor = Eigen::Matrix; // Using shorthand for `Vector` with scalar type. diff --git a/flightlib/include/flightlib/envs/env_base.hpp b/flightlib/include/flightlib/envs/env_base.hpp index fae3954b6e..d4a9c843e2 100644 --- a/flightlib/include/flightlib/envs/env_base.hpp +++ b/flightlib/include/flightlib/envs/env_base.hpp @@ -22,7 +22,7 @@ class EnvBase { virtual ~EnvBase() = 0; // (pure virtual) public methods (has to be implemented by child classes) - virtual bool reset(Ref> obs) = 0; + virtual void reset(Ref> obs) = 0; virtual Scalar step(Ref> act, Ref> obs) = 0; virtual bool getObs(Ref> obs) = 0; virtual bool setFlightmare(bool on) = 0; diff --git a/flightlib/include/flightlib/envs/quadrotor_env/quadrotor_env.hpp b/flightlib/include/flightlib/envs/quadrotor_env/quadrotor_env.hpp index 62ec73840e..3efa6e0406 100644 --- a/flightlib/include/flightlib/envs/quadrotor_env/quadrotor_env.hpp +++ b/flightlib/include/flightlib/envs/quadrotor_env/quadrotor_env.hpp @@ -22,7 +22,7 @@ class QuadrotorEnv final : public EnvBase { ~QuadrotorEnv(); // - public OpenAI-gym-style functions - bool reset(Ref> obs) override; + void reset(Ref> obs) override; Scalar step(Ref> act, Ref> obs) override; // - public set functions diff --git a/flightlib/include/flightlib/envs/test_env.hpp b/flightlib/include/flightlib/envs/test_env.hpp index 2ea6749224..7a3cace054 100644 --- a/flightlib/include/flightlib/envs/test_env.hpp +++ b/flightlib/include/flightlib/envs/test_env.hpp @@ -1,5 +1,11 @@ #pragma once +// pybind11 +// #include +// #include +// #include +#include + #include "flightlib/envs/env_base.hpp" #include "flightlib/envs/quadrotor_env/quadrotor_env.hpp" @@ -9,8 +15,13 @@ template class TestEnv { public: // constructor & deconstructor - explicit TestEnv(); + TestEnv(); ~TestEnv(); + + void reset( + Eigen::Ref< + Eigen::Matrix> + obs); }; } // namespace flightlib diff --git a/flightlib/include/flightlib/envs/vec_env.hpp b/flightlib/include/flightlib/envs/vec_env.hpp index fef3265089..0fb570c09a 100644 --- a/flightlib/include/flightlib/envs/vec_env.hpp +++ b/flightlib/include/flightlib/envs/vec_env.hpp @@ -17,7 +17,8 @@ namespace flightlib { template class VecEnv { public: - explicit VecEnv(std::string cfg_path); + VecEnv(); + VecEnv(const std::string& cfg_path); ~VecEnv(); // - public OpenAI-gym style functions for vectorized environment @@ -36,6 +37,9 @@ class VecEnv { inline int getActDim(void) { return act_dim_; }; inline int getExtraInfoDim(void) { return extra_info_names_.size(); }; inline int getNumOfEnvs(void) { return envs_.size(); }; + inline std::vector& getExtraInfoNames() { + return extra_info_names_; + }; // - auxiliary functions void isTerminalState(Ref> terminal_state); @@ -55,6 +59,7 @@ class VecEnv { // std::vector> envs_; std::vector extra_info_names_; + std::unique_ptr logger_; // auxiliar variables diff --git a/flightlib/src/envs/quadrotor_env/quadrotor_env.cpp b/flightlib/src/envs/quadrotor_env/quadrotor_env.cpp index a8113bc4a9..e15ef7e843 100644 --- a/flightlib/src/envs/quadrotor_env/quadrotor_env.cpp +++ b/flightlib/src/envs/quadrotor_env/quadrotor_env.cpp @@ -6,12 +6,11 @@ QuadrotorEnv::QuadrotorEnv() : EnvBase() { // define input and output dimension for the environment obs_dim_ = CtlObsAct::kObsSize; act_dim_ = CtlObsAct::kActSize; - std::cout << "Quadrotor Env." << std::endl; } QuadrotorEnv::~QuadrotorEnv() {} -bool QuadrotorEnv::reset(const Ref> obs) { +void QuadrotorEnv::reset(const Ref> obs) { quad_state_.setZero(); // randomly reset the quadrotor state @@ -31,7 +30,6 @@ bool QuadrotorEnv::reset(const Ref> obs) { // obtain observations getObs(obs); - return true; } bool QuadrotorEnv::getObs(Ref> obs) { diff --git a/flightlib/src/envs/test_env.cpp b/flightlib/src/envs/test_env.cpp index 316f62b2e6..d69d264923 100644 --- a/flightlib/src/envs/test_env.cpp +++ b/flightlib/src/envs/test_env.cpp @@ -11,6 +11,14 @@ TestEnv::TestEnv() { template TestEnv::~TestEnv() {} +template +void TestEnv::reset( + Eigen::Ref< + Eigen::Matrix> + obs) { + obs << 1, 2, 3, 4, 5, 6, 7, 8, 9; +} + template class TestEnv; } // namespace flightlib diff --git a/flightlib/src/envs/vec_env.cpp b/flightlib/src/envs/vec_env.cpp index abb596f795..170a02e7b3 100644 --- a/flightlib/src/envs/vec_env.cpp +++ b/flightlib/src/envs/vec_env.cpp @@ -3,20 +3,24 @@ namespace flightlib { template -VecEnv::VecEnv(const std::string cfg_path) { +VecEnv::VecEnv() + : VecEnv(getenv("FLIGHTMARE_PATH") + + std::string("/flightlib/configs/quadrotor_env.yaml")) {} + +template +VecEnv::VecEnv(const std::string& cfg_path) { // load environment configuration - cfg_ = YAML::Load(cfg_path); + cfg_ = YAML::LoadFile(cfg_path); // logger logger_ = std::make_unique("VecEnv"); - // - if (cfg_["env"]["render"]) - unity_render_ = cfg_["env"]["render"].template as(); - logger_->info("environment created."); + if (cfg_["env"]["render"]) { + unity_render_ = cfg_["env"]["render"].as(); + } - omp_set_num_threads(cfg_["env"]["num_threads"].template as()); - num_envs_ = cfg_["env"]["num_envs"].template as(); + omp_set_num_threads(cfg_["env"]["num_threads"].as()); + num_envs_ = cfg_["env"]["num_envs"].as(); // create & setup environments const bool render = false; @@ -29,6 +33,7 @@ VecEnv::VecEnv(const std::string cfg_path) { if (unity_render_) { // enable rendering for only one environment envs_[0]->setFlightmare(unity_render_); + logger_->info("Flightmare Unity Render is enabled!"); } obs_dim_ = envs_[0]->getObsDim(); @@ -40,8 +45,6 @@ VecEnv::VecEnv(const std::string cfg_path) { for (auto& re : envs_[0]->extra_info_) { extra_info_names_.push_back(re.first); } - - std::cout << "Vec Env" << std::endl; } template @@ -123,6 +126,9 @@ void VecEnv::connectFlightmare(void) {} template void VecEnv::disconnectFlightmare(void) {} +template +void VecEnv::curriculumUpdate(void) {} + // IMPORTANT. Otherwise: // Segmentation fault (core dumped) template class VecEnv; diff --git a/flightlib/src/wrapper/pybind_wrapper.cpp b/flightlib/src/wrapper/pybind_wrapper.cpp index 85de68903c..ee9f26c3f0 100644 --- a/flightlib/src/wrapper/pybind_wrapper.cpp +++ b/flightlib/src/wrapper/pybind_wrapper.cpp @@ -1,5 +1,6 @@ // pybind11 +#include #include #include @@ -14,18 +15,21 @@ using namespace flightlib; PYBIND11_MODULE(flightgym, m) { py::class_>(m, "QuadrotorEnv_v1") - .def(py::init()) + .def(py::init<>()) + .def(py::init()) .def("reset", &VecEnv::reset) .def("step", &VecEnv::step) .def("testStep", &VecEnv::testStep) .def("setSeed", &VecEnv::setSeed) .def("close", &VecEnv::close) .def("isTerminalState", &VecEnv::isTerminalState) + .def("curriculumUpdate", &VecEnv::curriculumUpdate) .def("connectFlightmare", &VecEnv::connectFlightmare) .def("disconnectFlightmare", &VecEnv::disconnectFlightmare) .def("getNumOfEnvs", &VecEnv::getNumOfEnvs) .def("getObsDim", &VecEnv::getObsDim) .def("getActDim", &VecEnv::getActDim) + .def("getExtraInfoNames", &VecEnv::getExtraInfoNames) .def("__repr__", [](const VecEnv& a) { return "RPG Drone Racing Environment"; }); @@ -46,5 +50,6 @@ PYBIND11_MODULE(flightgym, m) { py::class_>(m, "TestEnv_v0") .def(py::init<>()) + .def("reset", &TestEnv::reset) .def("__repr__", [](const TestEnv& a) { return "Test Env"; }); } \ No newline at end of file diff --git a/flightlib/test.py b/flightlib/test.py index 5a6166c66c..ca6fea2032 100644 --- a/flightlib/test.py +++ b/flightlib/test.py @@ -1,11 +1,20 @@ +import numpy as np + from flightgym import TestEnv_v0 from flightgym import QuadrotorEnv_v0 from flightgym import QuadrotorEnv_v1 def main(): test_env = TestEnv_v0() - quad_env0 = QuadrotorEnv_v0() + # quad_env0 = QuadrotorEnv_v0() + obs = np.zeros(shape=(3, 3), dtype=np.float32) + test_env.reset(obs) + print(obs) + quad_env1 = QuadrotorEnv_v1() + obs = np.zeros(shape=(1, 9), dtype=np.float32) + quad_env1.reset(obs) + print(obs) if __name__ == "__main__": main() diff --git a/flightlib/tests/envs/quadrotor_env.cpp b/flightlib/tests/envs/quadrotor_env.cpp index 19b8492772..6a790bc2d3 100644 --- a/flightlib/tests/envs/quadrotor_env.cpp +++ b/flightlib/tests/envs/quadrotor_env.cpp @@ -15,16 +15,9 @@ TEST(QuadrotorEnv, Constructor) { Logger logger("QuadrotorEnv"); logger.info("Environment configuration path \"%s\".", config_path.c_str()); - std::string env_path = getenv("HOME"); - logger.info("Environment configuration path \"%s\".", env_path.c_str()); - - std::ofstream file; - file.open(config_path); - std::cout << file.is_open() << std::endl; - // YAML::Node cfg = YAML::LoadFile(config_path); - QuadrotorEnv env(); + QuadrotorEnv env; // QuadState quad_state; diff --git a/flightlib/tests/envs/vec_env.cpp b/flightlib/tests/envs/vec_env.cpp new file mode 100644 index 0000000000..4c2db6babe --- /dev/null +++ b/flightlib/tests/envs/vec_env.cpp @@ -0,0 +1,25 @@ +#include "flightlib/envs/vec_env.hpp" +#include "flightlib/envs/quadrotor_env/quadrotor_env.hpp" + +#include +#include +#include +#include + +using namespace flightlib; + +TEST(VecEnv, Constructor) { + std::string config_path = + getenv("FLIGHTMARE_PATH") + + std::string("/flightlib/configs/quadrotor_env.yaml"); + + Logger logger("VecEnv TEST"); + logger.info("Environment configuration path \"%s\".", config_path.c_str()); + + VecEnv env(config_path); + + env.setSeed(10); + + int obs_dim = env.getObsDim(); + std::cout << "obs_dim : " << obs_dim << std::endl; +} \ No newline at end of file diff --git a/flightrl/build/lib/rpg_baselines/__init__.py b/flightrl/build/lib/rpg_baselines/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/flightrl/rpg_baselines/common/policies.py b/flightrl/rpg_baselines/common/policies.py index 484ef20443..d24b18b693 100644 --- a/flightrl/rpg_baselines/common/policies.py +++ b/flightrl/rpg_baselines/common/policies.py @@ -12,7 +12,7 @@ # MultiCategoricalProbabilityDistribution, DiagGaussianProbabilityDistribution, BernoulliProbabilityDistribution from stable_baselines.common.input import observation_input # -from alpha_baselines.common.distributions import make_proba_dist_type, CategoricalProbabilityDistribution, \ +from rpg_baselines.common.distributions import make_proba_dist_type, CategoricalProbabilityDistribution, \ MultiCategoricalProbabilityDistribution, DiagGaussianProbabilityDistribution, BernoulliProbabilityDistribution diff --git a/flightrl/rpg_baselines/envs/vec_env_wrapper.py b/flightrl/rpg_baselines/envs/vec_env_wrapper.py index a729803d3a..3d46173e8c 100644 --- a/flightrl/rpg_baselines/envs/vec_env_wrapper.py +++ b/flightrl/rpg_baselines/envs/vec_env_wrapper.py @@ -2,13 +2,13 @@ from gym import spaces from stable_baselines.common.vec_env import VecEnv -class AlphaGymVec(VecEnv): +class FlightEnvVec(VecEnv): # def __init__(self, impl): self.wrapper = impl - self.wrapper.init() self.num_obs = self.wrapper.getObsDim() self.num_acts = self.wrapper.getActDim() + print(self.num_obs, self.num_acts) self._observation_space = spaces.Box( np.ones(self.num_obs) * -np.Inf, np.ones(self.num_obs) * np.Inf, dtype=np.float32) diff --git a/flightrl/run_drone_control.py b/flightrl/run_drone_control.py index e69de29bb2..57630ce790 100644 --- a/flightrl/run_drone_control.py +++ b/flightrl/run_drone_control.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 + +import os +import math +import argparse +import tensorflow as tf +import numpy as np + +# +from stable_baselines import logger + +# +from rpg_baselines.common.policies import MlpPolicy +from rpg_baselines.common.util import ConfigurationSaver, TensorboardLauncher +from rpg_baselines.ppo.ppo2 import PPO2 +from rpg_baselines.envs import vec_env_wrapper as wrapper +# +from flightgym import QuadrotorEnv_v1 + +# +def parser(): + parser = argparse.ArgumentParser() + parser.add_argument('--quad_env_cfg', type=str, default=os.path.abspath("../../configs/env.yaml"), + help='configuration file of the quad environment') + parser.add_argument('--train', type=int, default=1, + help="To train new model or simply test pre-trained model") + parser.add_argument('--save_dir', type=str, default=os.path.dirname(os.path.realpath(__file__)), + help="Directory where to save the checkpoints and training metrics") + parser.add_argument('--env', type=str, default="QuadrotorEnv_v1", + help="environment name") + parser.add_argument('--seed', type=int, default=0, + help="Random seed") + parser.add_argument('--load_dir', type=str, + help="Directory where to load weights") + parser.add_argument('-m', '--mode', type=str, default='train', + help='set mode either train or test') + parser.add_argument('-w', '--weight', type=str, default='./saved/2020-07-23-15-41-33.zip', + help='trained weight path') + return parser + +def main(): + args = parser().parse_args() + mode = args.mode + + env = wrapper.FlightEnvVec(QuadrotorEnv_v1()) + + # set random seed + env.seed(args.seed) + np.random.seed(args.seed) + tf.set_random_seed(args.seed) + + # + if mode == 'train': + # save the configuration and other files + rsg_root = os.path.dirname(os.path.abspath(__file__)) + log_dir = rsg_root + '/saved' + saver = ConfigurationSaver(log_dir=log_dir) + model = PPO2( + tensorboard_log=saver.data_dir, + policy=MlpPolicy, # check activation function + policy_kwargs=dict(net_arch=[dict(pi=[128, 128], vf=[128, 128])], act_fun=tf.nn.relu), + env=env, + lam=0.95, + gamma=0.99, # lower 0.9 ~ 0.99 + # n_steps=math.floor(cfg['env']['max_time'] / cfg['env']['ctl_dt']), + n_steps=250, + ent_coef=0.00, + learning_rate=3e-4, + vf_coef=0.5, + max_grad_norm=0.5, + nminibatches=1, + noptepochs=10, + cliprange=0.2, + verbose=1, + ) + + # tensorboard + # Make sure that your chrome browser is already on. + # TensorboardLauncher(saver.data_dir + '/PPO2_1') + + # PPO run + # Originally the total timestep is 5 x 10^8 + # 10 zeros for nupdates to be 4000 + # 1000000000 is 2000 iterations and so + # 2000000000 is 4000 iterations. + logger.configure(folder=saver.data_dir) + model.learn( + total_timesteps=int(25000000), + log_dir=saver.data_dir, logger=logger) + model.save(saver.data_dir) + + # # Testing mode with a trained weight + # else: + # model = PPO2.load(args.weight) + # test_ppo2(env, model, render=True) + +if __name__ == "__main__": + main()