pybind works, fixed error in cmakelist

.gitignore

@@ -6,3 +6,5 @@ dist
 *.egg-info
 *.so
 __pycache__
+build
+saved

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)

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)

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 ""

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
 

flightlib/include/flightlib/bridges/unity_bridge.hpp

@@ -8,7 +8,7 @@
 #include <string>
 #include <unordered_map>
 
-// Include ZMQ bindings for comms with Unity.
+// Include ZMQ bindings for communications with Unity.
 #include <zmqpp/zmqpp.hpp>
 
 // flightlib

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<int rows = Dynamic, int cols = rows>
 using Matrix = Eigen::Matrix<Scalar, rows, cols>;
 
 // Using shorthand for `Matrix<ros, cols>` with scalar type.
-template<int rows = Dynamic, int cols = rows>
+template<int rows = Dynamic, int cols = Dynamic>
 using MatrixRowMajor = Eigen::Matrix<Scalar, rows, cols, Eigen::RowMajor>;
 
 // Using shorthand for `Vector<ros>` with scalar type.

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<Vector<>> obs) = 0;
+  virtual void reset(Ref<Vector<>> obs) = 0;
   virtual Scalar step(Ref<Vector<>> act, Ref<Vector<>> obs) = 0;
   virtual bool getObs(Ref<Vector<>> obs) = 0;
   virtual bool setFlightmare(bool on) = 0;

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<Vector<>> obs) override;
+  void reset(Ref<Vector<>> obs) override;
   Scalar step(Ref<Vector<>> act, Ref<Vector<>> obs) override;
 
   // - public set functions

flightlib/include/flightlib/envs/test_env.hpp

@@ -1,5 +1,11 @@
 #pragma once
 
+// pybind11
+// #include <pybind11/eigen.h>
+// #include <pybind11/pybind11.h>
+// #include <pybind11/stl.h>
+#include <eigen3/Eigen/Eigen>
+
 #include "flightlib/envs/env_base.hpp"
 #include "flightlib/envs/quadrotor_env/quadrotor_env.hpp"
 
@@ -9,8 +15,13 @@ template<typename EnvBase>
 class TestEnv {
  public:
   // constructor & deconstructor
-  explicit TestEnv();
+  TestEnv();
   ~TestEnv();
+
+  void reset(
+    Eigen::Ref<
+      Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      obs);
 };
 
 }  // namespace flightlib

flightlib/include/flightlib/envs/vec_env.hpp

@@ -17,7 +17,8 @@ namespace flightlib {
 template<typename EnvBase>
 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<std::string>& getExtraInfoNames() {
+    return extra_info_names_;
+  };
 
   // - auxiliary functions
   void isTerminalState(Ref<BoolVector<>> terminal_state);
@@ -55,6 +59,7 @@ class VecEnv {
   //
   std::vector<std::unique_ptr<EnvBase>> envs_;
   std::vector<std::string> extra_info_names_;
+
   std::unique_ptr<Logger> logger_;
 
   // auxiliar variables

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<Vector<>> obs) {
+void QuadrotorEnv::reset(const Ref<Vector<>> obs) {
   quad_state_.setZero();
 
   // randomly reset the quadrotor state
@@ -31,7 +30,6 @@ bool QuadrotorEnv::reset(const Ref<Vector<>> obs) {
 
   // obtain observations
   getObs(obs);
-  return true;
 }
 
 bool QuadrotorEnv::getObs(Ref<Vector<>> obs) {

flightlib/src/envs/test_env.cpp

@@ -11,6 +11,14 @@ TestEnv<EnvBase>::TestEnv() {
 template<typename EnvBase>
 TestEnv<EnvBase>::~TestEnv() {}
 
+template<typename EnvBase>
+void TestEnv<EnvBase>::reset(
+  Eigen::Ref<
+    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+    obs) {
+  obs << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+}
+
 template class TestEnv<QuadrotorEnv>;
 
 }  // namespace flightlib

flightlib/src/envs/vec_env.cpp

@@ -3,20 +3,24 @@
 namespace flightlib {
 
 template<typename EnvBase>
-VecEnv<EnvBase>::VecEnv(const std::string cfg_path) {
+VecEnv<EnvBase>::VecEnv()
+  : VecEnv(getenv("FLIGHTMARE_PATH") +
+           std::string("/flightlib/configs/quadrotor_env.yaml")) {}
+
+template<typename EnvBase>
+VecEnv<EnvBase>::VecEnv(const std::string& cfg_path) {
   // load environment configuration
-  cfg_ = YAML::Load(cfg_path);
+  cfg_ = YAML::LoadFile(cfg_path);
 
   // logger
   logger_ = std::make_unique<Logger>("VecEnv");
 
-  //
-  if (cfg_["env"]["render"])
-    unity_render_ = cfg_["env"]["render"].template as<bool>();
-  logger_->info("environment created.");
+  if (cfg_["env"]["render"]) {
+    unity_render_ = cfg_["env"]["render"].as<bool>();
+  }
 
-  omp_set_num_threads(cfg_["env"]["num_threads"].template as<int>());
-  num_envs_ = cfg_["env"]["num_envs"].template as<int>();
+  omp_set_num_threads(cfg_["env"]["num_threads"].as<int>());
+  num_envs_ = cfg_["env"]["num_envs"].as<int>();
 
   // create & setup environments
   const bool render = false;
@@ -29,6 +33,7 @@ VecEnv<EnvBase>::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<EnvBase>::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<typename EnvBase>
@@ -123,6 +126,9 @@ void VecEnv<EnvBase>::connectFlightmare(void) {}
 template<typename EnvBase>
 void VecEnv<EnvBase>::disconnectFlightmare(void) {}
 
+template<typename EnvBase>
+void VecEnv<EnvBase>::curriculumUpdate(void) {}
+
 // IMPORTANT. Otherwise:
 // Segmentation fault (core dumped)
 template class VecEnv<QuadrotorEnv>;

flightlib/src/wrapper/pybind_wrapper.cpp

@@ -1,5 +1,6 @@
 
 // pybind11
+#include <pybind11/eigen.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
@@ -14,18 +15,21 @@ using namespace flightlib;
 
 PYBIND11_MODULE(flightgym, m) {
   py::class_<VecEnv<QuadrotorEnv>>(m, "QuadrotorEnv_v1")
-    .def(py::init<std::string>())
+    .def(py::init<>())
+    .def(py::init<const std::string&>())
     .def("reset", &VecEnv<QuadrotorEnv>::reset)
     .def("step", &VecEnv<QuadrotorEnv>::step)
     .def("testStep", &VecEnv<QuadrotorEnv>::testStep)
     .def("setSeed", &VecEnv<QuadrotorEnv>::setSeed)
     .def("close", &VecEnv<QuadrotorEnv>::close)
     .def("isTerminalState", &VecEnv<QuadrotorEnv>::isTerminalState)
+    .def("curriculumUpdate", &VecEnv<QuadrotorEnv>::curriculumUpdate)
     .def("connectFlightmare", &VecEnv<QuadrotorEnv>::connectFlightmare)
     .def("disconnectFlightmare", &VecEnv<QuadrotorEnv>::disconnectFlightmare)
     .def("getNumOfEnvs", &VecEnv<QuadrotorEnv>::getNumOfEnvs)
     .def("getObsDim", &VecEnv<QuadrotorEnv>::getObsDim)
     .def("getActDim", &VecEnv<QuadrotorEnv>::getActDim)
+    .def("getExtraInfoNames", &VecEnv<QuadrotorEnv>::getExtraInfoNames)
     .def("__repr__", [](const VecEnv<QuadrotorEnv>& a) {
       return "RPG Drone Racing Environment";
     });
@@ -46,5 +50,6 @@ PYBIND11_MODULE(flightgym, m) {
 
   py::class_<TestEnv<QuadrotorEnv>>(m, "TestEnv_v0")
     .def(py::init<>())
+    .def("reset", &TestEnv<QuadrotorEnv>::reset)
     .def("__repr__", [](const TestEnv<QuadrotorEnv>& a) { return "Test Env"; });
 }

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()

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;

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 <gtest/gtest.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+#include <iostream>
+
+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<QuadrotorEnv> env(config_path);
+
+  env.setSeed(10);
+
+  int obs_dim = env.getObsDim();
+  std::cout << "obs_dim : " << obs_dim << std::endl;
+}

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