use cmd feedthrough control mode for RL task

flightlib/cmake/pybind11.cmake

@@ -6,18 +6,19 @@ set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING})
 
 configure_file(
   cmake/pybind11_download.cmake
-  ${PROJECT_SOURCE_DIR}/externals/pybind11/CMakeLists.txt)
+  ${PROJECT_SOURCE_DIR}/externals/pybind11-download/CMakeLists.txt)
 
-execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . 
   RESULT_VARIABLE result
-  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11-download
   OUTPUT_QUIET)
 if(result)
-  message(FATAL_ERROR "Download of Pybind11 failed: ${result}")
+  message(FATAL_ERROR "Cmake Step for Pybind11 failed: ${result}")
 endif()
+
 execute_process(COMMAND ${CMAKE_COMMAND} --build .
   RESULT_VARIABLE result
-  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/pybind11-download
   OUTPUT_QUIET)
 if(result)
   message(FATAL_ERROR "Build step for eigen failed: ${result}")

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           v2.4.3
+  GIT_TAG           master 
   SOURCE_DIR        "${PROJECT_SOURCE_DIR}/externals/pybind11-src"
   BINARY_DIR        "${PROJECT_SOURCE_DIR}/externals/pybind11-bin"
   CONFIGURE_COMMAND ""

flightlib/cmake/yaml.cmake

@@ -5,7 +5,7 @@ configure_file(
   cmake/yaml_download.cmake
   ${PROJECT_SOURCE_DIR}/externals/yaml-download/CMakeLists.txt)
 
-execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . 
   RESULT_VARIABLE result
   WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/externals/yaml-download
   OUTPUT_QUIET

flightlib/configs/quadrotor_env.yaml

@@ -6,7 +6,7 @@ quadrotor_dynamics:
   mass: 0.73
   arm_l: 0.17
   motor_omega_min: 150.0 # motor rpm min
-  motor_omega_max: 2000.0 # motor rpm max
+  motor_omega_max: 3000.0 # motor rpm max
   motor_tau: 0.0001 # motor step response
   thrust_map: [1.3298253500372892e-06, 0.0038360810526746033, -1.7689986848125325]
   kappa: 0.016 # rotor drag coeff

flightlib/include/flightlib/envs/quadrotor_env/quadrotor_env.hpp

@@ -86,10 +86,10 @@ class QuadrotorEnv final : public EnvBase {
   Vector<CtlObsAct::kObsSize> goal_state_;
 
   // action and observation normalization (for learning)
-  Vector<CtlObsAct::kActSize> act_mean_{-Gz, 0.0, 0.0, 0.0};
-  Vector<CtlObsAct::kActSize> act_std_{10.0, 6.0, 6.0, 6.0};
-  // Vector<CtlObsAct::kActSize> act_mean_{2.5, 2.5, 2.5, 2.5};
-  // Vector<CtlObsAct::kActSize> act_std_{6.0, 6.0, 6.0, 6.0};
+  // Vector<CtlObsAct::kActSize> act_mean_{-Gz, 0.0, 0.0, 0.0};
+  // Vector<CtlObsAct::kActSize> act_std_{10.0, 6.0, 6.0, 6.0};
+  Vector<CtlObsAct::kActSize> act_mean_;
+  Vector<CtlObsAct::kActSize> act_std_;
   Vector<CtlObsAct::kObsSize> obs_mean_ = Vector<CtlObsAct::kObsSize>::Zero();
   Vector<CtlObsAct::kObsSize> obs_std_ = Vector<CtlObsAct::kObsSize>::Ones();
 

flightlib/include/flightlib/objects/quadrotor.hpp

@@ -85,7 +85,7 @@ class Quadrotor : ObjectBase {
 
   // P gain for body-rate control
   const Matrix<3, 3> Kinv_ang_vel_tau_ =
-    Vector<3>(1.0 / 16.6, 1.0 / 16.6, 1.0 / 5.0).asDiagonal();
+    Vector<3>(16.6, 16.6, 5.0).asDiagonal();
   // gravity
   const Vector<3> gz_{0.0, 0.0, Gz};
 

flightlib/setup.py

@@ -1,4 +1,5 @@
 import os
+import glob
 import shutil
 import re
 import sys
@@ -18,34 +19,46 @@ def __init__(self, name, sourcedir=''):
 
 class CMakeBuild(build_ext):
     def run(self):
+        FLIGHTLIB_EXTERNAL_FILES = os.environ["FLIGHTMARE_PATH"] + \
+            "/flightlib/externals/"
+        # remove cached external files
+        # a hack to solve some cmake error when using "pip install ."
+        try:
+            for i, p in enumerate(glob.glob(os.path.join(FLIGHTLIB_EXTERNAL_FILES, "*"))):
+                shutil.rmtree(p)
+                print("Removing some cache file: ", p)
+        except:
+            pass
         try:
             out = subprocess.check_output(['cmake', '--version'])
         except OSError:
             raise RuntimeError("CMake must be installed to build the following extensions: " +
                                ", ".join(e.name for e in self.extensions))
 
         if platform.system() == "Windows":
-            cmake_version = LooseVersion(re.search(r'version\s*([\d.]+)', out.decode()).group(1))
+            cmake_version = LooseVersion(
+                re.search(r'version\s*([\d.]+)', out.decode()).group(1))
             if cmake_version < '3.1.0':
                 raise RuntimeError("CMake >= 3.1.0 is required on Windows")
 
         for ext in self.extensions:
             self.build_extension(ext)
 
     def build_extension(self, ext):
-        extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name)))
+        extdir = os.path.abspath(os.path.dirname(
+            self.get_ext_fullpath(ext.name)))
         # required for auto-detection of auxiliary "native" libs
         if not extdir.endswith(os.path.sep):
             extdir += os.path.sep
-        FLIGHTLIB_EXTERNAL_FILES = os.environ["FLIGHTMARE_PATH"] + "/flightlib/externals/"
         cmake_args = ['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=' + extdir,
                       '-DPYTHON_EXECUTABLE=' + sys.executable]
 
         cfg = 'Debug' if self.debug else 'Release'
         build_args = ['--config', cfg]
 
         if platform.system() == "Windows":
-            cmake_args += ['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{}={}'.format(cfg.upper(), extdir)]
+            cmake_args += [
+                '-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{}={}'.format(cfg.upper(), extdir)]
             if sys.maxsize > 2**32:
                 cmake_args += ['-A', 'x64']
             build_args += ['--', '/m']
@@ -58,8 +71,11 @@ def build_extension(self, ext):
                                                               self.distribution.get_version())
         if not os.path.exists(self.build_temp):
             os.makedirs(self.build_temp)
-        subprocess.check_call(['cmake', ext.sourcedir] + cmake_args, cwd=self.build_temp, env=env)
-        subprocess.check_call(['cmake', '--build', '.'] + build_args, cwd=self.build_temp)
+        subprocess.check_call(['cmake', ext.sourcedir] +
+                              cmake_args, cwd=self.build_temp, env=env)
+        subprocess.check_call(['cmake', '--build', '.'] +
+                              build_args, cwd=self.build_temp)
+
 
 setup(
     name='flightgym',
@@ -69,8 +85,22 @@ def build_extension(self, ext):
     description='Flightmare: A Quadrotor Simulator.',
     long_description='',
     ext_modules=[CMakeExtension('flightlib')],
-    install_requires=['gym==0.10.9', 'ruamel.yaml', 'numpy', 'stable_baselines==2.8'],
+    install_requires=['gym==0.10.9', 'ruamel.yaml',
+                      'numpy', 'stable_baselines==2.8'],
     cmdclass=dict(build_ext=CMakeBuild),
     include_package_data=True,
     zip_safe=False,
 )
+
+# setup(name='flightgym',
+#       version='0.0.1',
+#       author="Yunlong Song",
+#       author_email='[email protected]',
+#       description="Flightmare: A Quadrotor Simulator",
+#       long_description='',
+#       packages=[''],
+#       package_dir={'': './build/'},
+#       package_data={'': ['flightgym.cpython-36m-x86_64-linux-gnu.so']},
+#       zip_fase=True,
+#       url=None,
+#       )

flightlib/src/envs/quadrotor_env/quadrotor_env.cpp

@@ -33,6 +33,11 @@ QuadrotorEnv::QuadrotorEnv(const std::string &cfg_path)
   obs_dim_ = CtlObsAct::kObsSize;
   act_dim_ = CtlObsAct::kActSize;
 
+
+  Scalar mass = quadrotor_.getMass();
+  act_mean_ = Vector<4>::Ones() * (-mass * Gz) / 4;
+  act_std_ = Vector<4>::Ones() * (-mass * 3 * Gz) / 4;
+
   // load parameters
   loadParam(cfg_);
 }
@@ -41,6 +46,7 @@ QuadrotorEnv::~QuadrotorEnv() {}
 
 bool QuadrotorEnv::reset(Ref<Vector<>> obs, const bool random) {
   quad_state_.setZero();
+  quad_act_.setZero();
 
   if (random) {
     // randomly reset the quadrotor state
@@ -66,9 +72,9 @@ bool QuadrotorEnv::reset(Ref<Vector<>> obs, const bool random) {
 
   // reset control command
   cmd_.t = 0.0;
-  // cmd_.thrusts.setZero();
-  cmd_.collective_thrust = 0.0;
-  cmd_.omega = Vector<3>::Zero();
+  cmd_.thrusts.setZero();
+  // cmd_.collective_thrust = 0.0;
+  // cmd_.omega = Vector<3>::Zero();
 
   // obtain observations
   getObs(obs);
@@ -90,8 +96,9 @@ bool QuadrotorEnv::getObs(Ref<Vector<>> obs) {
 Scalar QuadrotorEnv::step(const Ref<Vector<>> act, Ref<Vector<>> obs) {
   quad_act_ = act.cwiseProduct(act_std_) + act_mean_;
   cmd_.t += sim_dt_;
-  cmd_.collective_thrust = quad_act_(0);
-  cmd_.omega = quad_act_.segment<3>(CtlObsAct::kOmegaX);
+  cmd_.thrusts = quad_act_;
+  // cmd_.collective_thrust = quad_act_(0);
+  // cmd_.omega = quad_act_.segment<3>(CtlObsAct::kOmegaX);
 
   // simulate quadrotor
   quadrotor_.run(cmd_, sim_dt_);
@@ -107,13 +114,18 @@ Scalar QuadrotorEnv::step(const Ref<Vector<>> act, Ref<Vector<>> obs) {
 
   Scalar total_reward = stage_reward + act_reward;
 
-  if (quad_state_.p(QS::POSZ) <= 0.02) {
-    total_reward -= 0.02;
+  if (quad_state_.x(QS::POSZ) <= 0.02) {
+    total_reward = total_reward - 0.02;
   }
+
   return total_reward;
 }
 
 bool QuadrotorEnv::isTerminalState(Scalar &reward) {
+  // if (quad_state_.x(QS::POSZ) <= 0.02) {
+  //   reward = -0.02;
+  //   return true;
+  // }
   reward = 0.0;
   return false;
 }

flightlib/src/objects/quadrotor.cpp

@@ -40,6 +40,8 @@ bool Quadrotor::run(const Scalar ctl_dt) {
 
   QuadState old_state = state_;
   QuadState next_state = state_;
+
+  // time
   const Scalar max_dt = integrator_ptr_->dtMax();
   Scalar remain_ctl_dt = ctl_dt;
 
@@ -65,7 +67,6 @@ bool Quadrotor::run(const Scalar ctl_dt) {
 
     remain_ctl_dt -= sim_dt;
   }
-
   state_.t += ctl_dt;
   constrainInWorldBox(old_state);
   return true;
@@ -180,6 +181,7 @@ bool Quadrotor::constrainInWorldBox(const QuadState &old_state) {
     // reset angular velocity to zero
     state_.w << 0.0, 0.0, 0.0;
   }
+  return true;
 }
 
 bool Quadrotor::getState(QuadState *const state) const {

flightlib/tests/envs/vec_env.cpp

@@ -84,7 +84,6 @@ TEST(VecEnv, ResetEnv) {
   EXPECT_TRUE(vec_env.reset(obs));
   EXPECT_TRUE(obs.allFinite());
 
-
   obs.resize(vec_num_env, vec_obs_dim + 1);
   EXPECT_FALSE(vec_env.reset(obs));
 }
@@ -97,7 +96,7 @@ TEST(VecEnv, StepEnv) {
   const int num_envs = vec_env.getNumOfEnvs();
   const std::vector<std::string> extra_info_names = vec_env.getExtraInfoNames();
 
-  vec_env.setUnity(true);
+  // vec_env.setUnity(true);
 
   // reset the environment
   MatrixRowMajor<> obs, act, extra_info;
@@ -114,9 +113,9 @@ TEST(VecEnv, StepEnv) {
   EXPECT_TRUE(obs.allFinite());
 
   // test step function
-  act.setRandom();
-  act = act.cwiseMax(-1).cwiseMin(1);
   for (int i = 0; i < SIM_STEPS_N; i++) {
+    act.setRandom();
+    act = act.cwiseMax(-1).cwiseMin(1);
     vec_env.step(act, obs, reward, done, extra_info);
   }
   EXPECT_TRUE(act.allFinite());

flightlib/tests/flightgym/interface.py

@@ -15,7 +15,5 @@ def main():
   quad_env1.reset(obs)
   print(obs)
 
-  print(os.environ("FLIGHTMARE_PATH"))
-
 if __name__ == "__main__":
     main()

flightlib/tests/objects/quadrotor.cpp

@@ -182,7 +182,7 @@ TEST(Quadrotor, RunSimulatorBodyRate) {
   Command cmd;
   cmd.t = 0.0;
   cmd.collective_thrust = -Gz;
-  cmd.omega = Vector<3>::Zero();
+  cmd.omega << 0.0, 0.0, 0.0;
 
   for (int i = 0; i < SIM_STEPS_N; i++) {
     // run quadrotor simulator