Simplify the code and remove drone dynamics (unused)

README.md

@@ -29,7 +29,7 @@ Compared to giving expert demonstrations for imitation in imitation learning or
 
 ## Acknowledgements: 
 
-This project is developed based on the open-source simulator [Flightmare](https://github.com/uzh-rpg/flightmare) and the gradient computation is modified from [grad_traj_optimization](https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization), thanks for their excellent work!
+This project is developed based on the open-source simulator [Flightmare](https://github.com/uzh-rpg/flightmare), the gradient computation is modified from [grad_traj_optimization](https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization), and the signed distance field is built by [sdf_tools](https://github.com/UM-ARM-Lab/sdf_tools). thanks for their excellent work!
 
 ## Installation
 

flightlib/CMakeLists.txt

@@ -171,7 +171,6 @@ add_subdirectory(${PROJECT_SOURCE_DIR}/third_party/sdf_tools)
 # Create file lists for flightlib source
 file(GLOB_RECURSE FLIGHTLIB_SOURCES
 	src/bridges/*.cpp
-	src/dynamics/*.cpp
 	src/objects/*.cpp
 	src/sensors/*.cpp
 	src/sensors/*.cu

flightlib/include/flightlib/common/command.hpp

@@ -1,37 +0,0 @@
-
-#pragma once
-
-#include <cmath>
-
-#include "flightlib/common/types.hpp"
-
-namespace flightlib {
-
-struct Command {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  Command();
-
-  Command(const Scalar t, const Scalar thrust, const Vector<3>& omega);
-
-  Command(const Scalar t, const Vector<4>& thrusts);
-
-  bool valid() const;
-  bool isSingleRotorThrusts() const;
-  bool isRatesThrust() const;
-
-  /// time in [s]
-  Scalar t{NAN};
-
-  /// Collective mass-normalized thrust in [m/s^2]
-  Scalar collective_thrust{NAN};
-
-  /// Bodyrates in [rad/s]
-  Vector<3> omega{NAN, NAN, NAN};
-
-  /// Single rotor thrusts in [N]
-  Vector<4> thrusts{NAN, NAN, NAN, NAN};
-
-};
-
-}  // namespace flightlib

flightlib/include/flightlib/common/integrator_base.hpp

@@ -1,32 +0,0 @@
-#pragma once
-
-#include <functional>
-
-#include "flightlib/common/quad_state.hpp"
-#include "flightlib/common/types.hpp"
-
-namespace flightlib {
-
-class IntegratorBase {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- public:
-  using DynamicsFunction =
-    std::function<bool(const Ref<const Vector<>>, Ref<Vector<>>)>;
-  IntegratorBase(DynamicsFunction function, const Scalar dt_max = 1e-3);
-
-  bool integrate(const QuadState& initial, QuadState* const final) const;
-
-  bool integrate(const Ref<const Vector<>> initial, const Scalar dt,
-                 Ref<Vector<>> final) const;
-
-  virtual bool step(const Ref<const Vector<>> initial, const Scalar dt,
-                    Ref<Vector<>> final) const = 0;
-
-  Scalar dtMax() const;
-
- protected:
-  DynamicsFunction dynamics_;
-  Scalar dt_max_;
-};
-
-}  // namespace flightlib

flightlib/include/flightlib/common/integrator_euler.hpp

@@ -1,17 +0,0 @@
-#pragma once
-
-#include "flightlib/common/integrator_base.hpp"
-
-namespace flightlib {
-
-class IntegratorEuler : public IntegratorBase {
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  using IntegratorBase::DynamicsFunction;
-  using IntegratorBase::IntegratorBase;
-
-  bool step(const Ref<const Vector<>> initial, const Scalar dt,
-            Ref<Vector<>> final) const;
-};
-
-}  // namespace flightlib

flightlib/include/flightlib/common/integrator_rk4.hpp

@@ -1,17 +0,0 @@
-#pragma once
-
-#include "flightlib/common/integrator_base.hpp"
-
-namespace flightlib {
-
-class IntegratorRK4 : public IntegratorBase {
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  using IntegratorBase::DynamicsFunction;
-  using IntegratorBase::IntegratorBase;
-
-  bool step(const Ref<const Vector<>> initial, const Scalar dt,
-            Ref<Vector<>> final) const;
-};
-
-}  // namespace flightlib

flightlib/include/flightlib/dynamics/dynamics_base.hpp

@@ -1,21 +0,0 @@
-#pragma once
-
-#include "flightlib/common/types.hpp"
-
-namespace flightlib {
-
-class DynamicsBase {
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-   public:
-	using DynamicsFunction = std::function<bool(const Ref<const Vector<>>, Ref<Vector<>>)>;
-
-	DynamicsBase();
-	virtual ~DynamicsBase();
-
-	// public get function
-	virtual DynamicsFunction getDynamicsFunction() const = 0;
-
-   private:
-};
-
-}  // namespace flightlib

flightlib/include/flightlib/dynamics/quadrotor_dynamics.hpp

@@ -1,87 +0,0 @@
-#pragma once
-
-#include <yaml-cpp/yaml.h>
-
-#include <iostream>
-#include <memory>
-
-#include "flightlib/common/logger.hpp"
-#include "flightlib/common/math.hpp"
-#include "flightlib/common/quad_state.hpp"
-#include "flightlib/dynamics/dynamics_base.hpp"
-
-namespace flightlib {
-
-class QuadrotorDynamics : DynamicsBase {
-   public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-	QuadrotorDynamics(const Scalar mass = 1.0, const Scalar arm_l = 0.2);
-	~QuadrotorDynamics();
-
-	// dynamics function
-	bool dState(const QuadState& state, QuadState* derivative) const;
-	bool dState(const Ref<const Vector<QuadState::SIZE>> state, Ref<Vector<QuadState::SIZE>> derivative) const;
-
-	// public get function
-	DynamicsFunction getDynamicsFunction() const;
-
-	// help functions
-	bool valid() const;
-	bool updateParams(const YAML::Node& params);
-
-	// Helpers to apply limits.
-	Vector<4> clampThrust(const Vector<4> thrusts) const;
-	Scalar clampThrust(const Scalar thrust) const;
-	Scalar clampCollectiveThrust(const Scalar thrust) const;
-
-	Vector<4> clampMotorOmega(const Vector<4>& omega) const;
-	Vector<3> clampBodyrates(const Vector<3>& omega) const;
-
-	inline Scalar collective_thrust_min() const { return 4.0 * thrust_min_; }
-	inline Scalar collective_thrust_max() const { return 4.0 * thrust_max_; }
-
-	// Helpers for conversion
-	Vector<4> motorOmegaToThrust(const Vector<4>& omega) const;
-	Vector<4> motorThrustToOmega(const Vector<4>& thrusts) const;
-	Matrix<4, 4> getAllocationMatrix() const;
-
-	//
-	inline Scalar getMass(void) const { return mass_; };
-	inline Scalar getArmLength(void) const { return arm_l_; };
-	inline Scalar getMotorTauInv() const { return motor_tau_inv_; };
-	inline Matrix<3, 3> getJ(void) const { return J_; };
-	inline Matrix<3, 3> getJInv(void) const { return J_inv_; };
-	inline Vector<3> getOmegaMax(void) const { return omega_max_; };
-
-	bool setMass(const Scalar mass);
-	bool setArmLength(const Scalar arm_length);
-	bool setMotortauInv(const Scalar tau_inv);
-
-	friend std::ostream& operator<<(std::ostream& os, const QuadrotorDynamics& quad_dymaics);
-
-   private:
-	bool updateInertiaMarix();
-	Scalar mass_;
-	Scalar arm_l_;
-	Matrix<3, 4> t_BM_;
-	Matrix<3, 3> J_;
-	Matrix<3, 3> J_inv_;
-
-	// motors
-	Scalar motor_omega_min_;
-	Scalar motor_omega_max_;
-	Scalar motor_tau_inv_;
-
-	// Propellers
-	Vector<3> thrust_map_;
-	Scalar kappa_;
-	Scalar thrust_min_;
-	Scalar thrust_max_;
-	Scalar collective_thrust_min_;
-	Scalar collective_thrust_max_;
-
-	// Quadrotor limits
-	Vector<3> omega_max_;
-};
-
-}  // namespace flightlib

flightlib/include/flightlib/envs/quadrotor_env.hpp

@@ -12,7 +12,6 @@
 #include <iostream>
 // flightlib
 #include "flightlib/bridges/unity_bridge.hpp"
-#include "flightlib/common/command.hpp"
 #include "flightlib/common/logger.hpp"
 #include "flightlib/common/math.hpp"
 #include "flightlib/common/quad_state.hpp"

flightlib/include/flightlib/objects/object_base.hpp

@@ -1,19 +0,0 @@
-#pragma once
-
-#include "flightlib/common/types.hpp"
-
-namespace flightlib {
-
-class ObjectBase {
- public:
-  ObjectBase();
-  virtual ~ObjectBase();
-
-  // reset
-  virtual bool reset(void) = 0;
-
-  // run
-  virtual bool run(const Scalar dt) = 0;
-};
-
-}  // namespace flightlib

flightlib/include/flightlib/objects/quadrotor.hpp

@@ -2,7 +2,6 @@
 
 /*
     Explanation:
-        We retain this class but do not use most of its functionalities.
         For efficiency, we do not use the built-in dynamics model and instead
         directly use the desired attitude given by the controller as actual state.
         Because the proposed approach (YOPO) only focuses on the trajectory performance,
@@ -11,98 +10,58 @@
 
 #include <stdlib.h>
 
-#include "flightlib/common/command.hpp"
-#include "flightlib/common/integrator_rk4.hpp"
 #include "flightlib/common/types.hpp"
-#include "flightlib/dynamics/quadrotor_dynamics.hpp"
+#include "flightlib/common/quad_state.hpp"
-#include "flightlib/objects/object_base.hpp"
 #include "flightlib/sensors/imu.hpp"
 #include "flightlib/sensors/rgb_camera.hpp"
 
 namespace flightlib {
 
-class Quadrotor : ObjectBase {
+class Quadrotor {
    public:
 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-	Quadrotor(const std::string& cfg_path);
+	Quadrotor();
-	Quadrotor(const QuadrotorDynamics& dynamics = QuadrotorDynamics(1.0, 0.25));
 	~Quadrotor();
 
 	// reset
-	bool reset(void) override;
+	bool reset(void);
 	bool reset(const QuadState& state);
-	void init(void);
 
 	// run the quadrotor
 	bool setState(const Ref<Vector<>> p, const Ref<Vector<>> v, const Quaternion q_, const Ref<Vector<>> a_, const Scalar ctl_dt);
-	bool run(const Scalar dt) override;
-	bool run(const Command& cmd, const Scalar dt);
 	void runSimpleFlight(const Eigen::Vector3f& ref_acc, float ref_yaw, Eigen::Quaternionf& quat_des);
 
 	// public get functions
 	bool getState(QuadState* const state) const;
-	bool getMotorThrusts(Ref<Vector<4>> motor_thrusts) const;
-	bool getMotorOmega(Ref<Vector<4>> motor_omega) const;
-	bool getDynamics(QuadrotorDynamics* const dynamics) const;
-
-	const QuadrotorDynamics& getDynamics();
 	Vector<3> getSize(void) const;
 	Vector<3> getPosition(void) const;
 	Quaternion getQuaternion(void) const;
-	std::vector<std::shared_ptr<RGBCamera>> getCameras(void) const;
-	bool getCamera(const size_t cam_id, std::shared_ptr<RGBCamera> camera) const;
 	int getNumCamera() const;
 	bool getCollision() const;
+	std::vector<std::shared_ptr<RGBCamera>> getCameras(void) const;
+	bool getCamera(const size_t cam_id, std::shared_ptr<RGBCamera> camera) const;
+	float getSimT() { return state_.t; }
 
 	// public set functions
 	bool setState(const QuadState& state);
-	bool setCommand(const Command& cmd);
-	bool updateDynamics(const QuadrotorDynamics& dynamics);
 	bool addRGBCamera(std::shared_ptr<RGBCamera> camera);
-
+	inline void setSize(const Ref<Vector<3>> size) { size_ = size; };
-	// low-level controller
+	inline void setCollision(const bool collision) { collision_ = collision; };
-	Vector<4> runFlightCtl(const Scalar sim_dt, const Vector<3>& omega, const Command& cmd);
-
-	// simulate motors
-	void runMotors(const Scalar sim_dt, const Vector<4>& motor_thrust_des);
 
 	// constrain world box
 	bool setWorldBox(const Ref<Matrix<3, 2>> box);
 	bool constrainInWorldBox(const QuadState& old_state);
 
-	//
-	inline Scalar getMass(void) { return dynamics_.getMass(); };
-	inline void setSize(const Ref<Vector<3>> size) { size_ = size; };
-	inline void setCollision(const bool collision) { collision_ = collision; };
-
-	float getSimT() { return state_.t; }
-
    private:
-	// quadrotor dynamics, integrators
+	// quadrotor sensors
-	QuadrotorDynamics dynamics_;
 	IMU imu_;
-	std::unique_ptr<IntegratorRK4> integrator_ptr_;
 	std::vector<std::shared_ptr<RGBCamera>> rgb_cameras_;
 
-	// quad control command
-	Command cmd_;
-
 	// quad state
 	QuadState state_;
 	Vector<3> size_;
 	bool collision_;
 
-	// auxiliar variablers
-	Vector<4> motor_omega_;
-	Vector<4> motor_thrusts_;
-	Matrix<4, 4> B_allocation_;
-	Matrix<4, 4> B_allocation_inv_;
-
-	// P gain for body-rate control
-	const Matrix<3, 3> Kinv_ang_vel_tau_ = Vector<3>(16.6, 16.6, 5.0).asDiagonal();
-	// gravity
-	const Vector<3> gz_{0.0, 0.0, Gz};
-
 	// auxiliary variables
 	Matrix<3, 2> world_box_;
 };

flightlib/src/common/command.cpp

@@ -1,29 +0,0 @@
-#include "flightlib/common/command.hpp"
-
-
-namespace flightlib {
-
-Command::Command() {}
-
-Command::Command(const Scalar t, const Scalar thrust, const Vector<3>& omega)
-  : t(t), collective_thrust(thrust), omega(omega) {}
-
-Command::Command(const Scalar t, const Vector<4>& thrusts)
-  : t(t), thrusts(thrusts) {}
-
-bool Command::valid() const {
-  return std::isfinite(t) &&
-         ((std::isfinite(collective_thrust) && omega.allFinite()) ^
-          thrusts.allFinite());
-}
-
-bool Command::isSingleRotorThrusts() const {
-  return std::isfinite(t) && thrusts.allFinite();
-}
-
-bool Command::isRatesThrust() const {
-  return std::isfinite(t) && std::isfinite(collective_thrust) &&
-         omega.allFinite();
-}
-
-}  // namespace flightlib

flightlib/src/common/integrator_base.cpp

@@ -1,33 +0,0 @@
-#include "flightlib/common/integrator_base.hpp"
-
-namespace flightlib {
-
-IntegratorBase::IntegratorBase(IntegratorBase::DynamicsFunction function,
-                               const Scalar dt_max)
-  : dynamics_(function), dt_max_(dt_max) {}
-
-bool IntegratorBase::integrate(const QuadState& initial,
-                               QuadState* const final) const {
-  if (std::isnan(initial.t) || std::isnan(final->t)) return false;
-  if (initial.t >= final->t) return false;
-  return integrate(initial.x, final->t - initial.t, final->x);
-}
-
-bool IntegratorBase::integrate(const Ref<const Vector<>> initial,
-                               const Scalar dt, Ref<Vector<>> final) const {
-  Scalar dt_remaining = dt;
-  Vector<> state = initial;
-
-  do {
-    const Scalar dt_this = std::min(dt_remaining, dt_max_);
-    if (!step(state, dt_this, final)) return false;
-    state = final;
-    dt_remaining -= dt_this;
-  } while (dt_remaining > 0.0);
-
-  return true;
-}
-
-Scalar IntegratorBase::dtMax() const { return dt_max_; }
-
-}  // namespace flightlib

flightlib/src/common/integrator_euler.cpp

@@ -1,15 +0,0 @@
-#include "flightlib/common/integrator_euler.hpp"
-
-namespace flightlib {
-
-bool IntegratorEuler::step(const Ref<const Vector<>> initial, const Scalar dt,
-                           Ref<Vector<>> final) const {
-  Vector<> derivative(initial.rows());
-  if (!this->dynamics_(initial, derivative)) return false;
-
-  final = initial + dt * derivative;
-
-  return true;
-}
-
-}  // namespace flightlib

flightlib/src/common/integrator_rk4.cpp

@@ -1,34 +0,0 @@
-#include "flightlib/common/integrator_rk4.hpp"
-
-namespace flightlib {
-
-bool IntegratorRK4::step(const Ref<const Vector<>> initial, const Scalar dt,
-                         Ref<Vector<>> final) const {
-  static const Vector<4> rk4_sum_vec{1.0 / 6.0, 2.0 / 6.0, 2.0 / 6.0,
-                                     1.0 / 6.0};
-  Matrix<> k = Matrix<>::Zero(initial.rows(), 4);
-
-  final = initial;
-
-  // k_1
-  if (!this->dynamics_(final, k.col(0))) return false;
-
-  // k_2
-  final = initial + 0.5 * dt * k.col(0);
-  if (!this->dynamics_(final, k.col(1))) return false;
-
-  // k_3
-  final = initial + 0.5 * dt * k.col(1);
-  if (!this->dynamics_(final, k.col(2))) return false;
-
-  // k_4
-  final = initial + dt * k.col(2);
-  if (!this->dynamics_(final, k.col(3))) return false;
-
-
-  final = initial + dt * k * rk4_sum_vec;
-
-  return true;
-}
-
-}  // namespace flightlib

flightlib/src/dynamics/dynamics_base.cpp

@@ -1,9 +0,0 @@
-#include "flightlib/dynamics/dynamics_base.hpp"
-
-namespace flightlib {
-
-DynamicsBase::DynamicsBase() {}
-
-DynamicsBase::~DynamicsBase() {}
-
-}  // namespace flightlib

flightlib/src/dynamics/quadrotor_dynamics.cpp

@@ -1,228 +0,0 @@
-#include "flightlib/dynamics/quadrotor_dynamics.hpp"
-
-namespace flightlib {
-
-QuadrotorDynamics::QuadrotorDynamics(const Scalar mass, const Scalar arm_l)
-  : mass_(mass),
-    arm_l_(arm_l),
-    t_BM_(
-      arm_l * sqrt(0.5) *
-      (Matrix<3, 4>() << 1, -1, -1, 1, -1, -1, 1, 1, 0, 0, 0, 0).finished()),
-    J_(mass / 12.0 * arm_l * arm_l * Vector<3>(4.5, 4.5, 7).asDiagonal()),
-    J_inv_(J_.inverse()),
-    motor_omega_min_(150.0),
-    motor_omega_max_(2000.0),
-    motor_tau_inv_(1.0 / 0.05),
-    thrust_map_(1.3298253500372892e-06, 0.0038360810526746033,
-                -1.7689986848125325),
-    kappa_(0.016),
-    thrust_min_(0.0),
-    thrust_max_(motor_omega_max_ * motor_omega_max_ * thrust_map_(0) +
-                motor_omega_max_ * thrust_map_(1) + thrust_map_(2)),
-    collective_thrust_min_(4.0 * thrust_min_ / mass_),
-    collective_thrust_max_(4.0 * thrust_max_ / mass_),            
-    omega_max_(6.0, 6.0, 2.0) {}
-
-QuadrotorDynamics::~QuadrotorDynamics() {}
-
-bool QuadrotorDynamics::dState(const QuadState& state,
-                               QuadState* dstate) const {
-  return dState(state.x, dstate->x);
-}
-
-bool QuadrotorDynamics::dState(const Ref<const Vector<QuadState::SIZE>> state,
-                               Ref<Vector<QuadState::SIZE>> dstate) const {
-  if (!state.segment<QS::NDYM>(0).allFinite()) return false;
-
-  dstate.setZero();
-  //
-  const Vector<3> omega(state(QS::OMEX), state(QS::OMEY), state(QS::OMEZ));
-  const Quaternion q_omega(0, omega.x(), omega.y(), omega.z());
-  const Vector<3> body_torque = state.segment<QS::NTAU>(QS::TAU);
-
-  // linear velocity = dx / dt
-  dstate.segment<QS::NPOS>(QS::POS) = state.segment<QS::NVEL>(QS::VEL);
-
-  // differentiate quaternion = dq / dt
-  dstate.segment<QS::NATT>(QS::ATT) =
-    0.5 * Q_right(q_omega) * state.segment<QS::NATT>(QS::ATT);
-
-  // linear acceleration = dv / dt
-  dstate.segment<QS::NVEL>(QS::VEL) = state.segment<QS::NACC>(QS::ACC);
-
-  // angular accleration = domega / dt
-  dstate.segment<QS::NOME>(QS::OME) =
-    J_inv_ * (body_torque - omega.cross(J_ * omega));
-  //
-  return true;
-}
-
-QuadrotorDynamics::DynamicsFunction QuadrotorDynamics::getDynamicsFunction()
-  const {
-  return std::bind(
-    static_cast<bool (QuadrotorDynamics::*)(const Ref<const Vector<QS::SIZE>>,
-                                            Ref<Vector<QS::SIZE>>) const>(
-      &QuadrotorDynamics::dState),
-    this, std::placeholders::_1, std::placeholders::_2);
-}
-
-bool QuadrotorDynamics::valid() const {
-  bool check = true;
-
-  check &= mass_ > 0.0;
-  check &= mass_ < 100.0;  // limit maximum mass
-  check &= t_BM_.allFinite();
-  check &= J_.allFinite();
-  check &= J_inv_.allFinite();
-
-  check &= motor_omega_min_ >= 0.0;
-  check &= (motor_omega_max_ > motor_omega_min_);
-  check &= motor_tau_inv_ > 0.0;
-
-  check &= thrust_map_.allFinite();
-  check &= kappa_ > 0.0;
-  check &= thrust_min_ >= 0.0;
-  check &= (thrust_max_ > thrust_min_);
-
-  check &= (omega_max_.array() > 0).all();
-
-  return check;
-}
-
-Vector<4> QuadrotorDynamics::clampThrust(const Vector<4> thrusts) const {
-  return thrusts.cwiseMax(thrust_min_).cwiseMin(thrust_max_);
-}
-
-Scalar QuadrotorDynamics::clampThrust(const Scalar thrust) const {
-  return std::clamp(thrust, thrust_min_, thrust_max_);
-}
-
-Scalar QuadrotorDynamics::clampCollectiveThrust(const Scalar thrust) const {
-  return std::clamp(thrust, collective_thrust_min_, collective_thrust_max_);
-}
-
-Vector<4> QuadrotorDynamics::clampMotorOmega(const Vector<4>& omega) const {
-  return omega.cwiseMax(motor_omega_min_).cwiseMin(motor_omega_max_);
-}
-
-Vector<3> QuadrotorDynamics::clampBodyrates(const Vector<3>& omega) const {
-  return omega.cwiseMax(-omega_max_).cwiseMin(omega_max_);
-}
-
-Vector<4> QuadrotorDynamics::motorOmegaToThrust(const Vector<4>& omega) const {
-  const Matrix<4, 3> omega_poly =
-    (Matrix<4, 3>() << omega.cwiseProduct(omega), omega, Vector<4>::Ones())
-      .finished();
-  return omega_poly * thrust_map_;
-}
-
-Vector<4> QuadrotorDynamics::motorThrustToOmega(
-  const Vector<4>& thrusts) const {
-  const Scalar scale = 1.0 / (2.0 * thrust_map_[0]);
-  const Scalar offset = -thrust_map_[1] * scale;
-
-  const Array<4, 1> root =
-    (std::pow(thrust_map_[1], 2) -
-     4.0 * thrust_map_[0] * (thrust_map_[2] - thrusts.array()))
-      .sqrt();
-
-  return offset + scale * root;
-}
-
-Matrix<4, 4> QuadrotorDynamics::getAllocationMatrix() const {
-  return (Matrix<4, 4>() << Vector<4>::Ones().transpose(), t_BM_.topRows<2>(),
-          kappa_ * Vector<4>(1, -1, 1, -1).transpose())
-    .finished();
-}
-
-bool QuadrotorDynamics::setMass(const Scalar mass) {
-  if (mass < 0.0 || mass >= 100.) {
-    return false;
-  }
-  mass_ = mass;
-  // update inertial matrix and its inverse
-  updateInertiaMarix();
-  return true;
-}
-
-bool QuadrotorDynamics::setArmLength(const Scalar arm_length) {
-  if (arm_length < 0.0) {
-    return false;
-  }
-  arm_l_ = arm_length;
-  // update torque mapping matrix, inertial matrix and its inverse
-  updateInertiaMarix();
-  return true;
-}
-
-bool QuadrotorDynamics::setMotortauInv(const Scalar tau_inv) {
-  if (tau_inv < 1.0) {
-    return false;
-  }
-  motor_tau_inv_ = tau_inv;
-  return true;
-}
-
-bool QuadrotorDynamics::updateParams(const YAML::Node& params) {
-  if (params["quadrotor_dynamics"]) {
-    // load parameters from a yaml configuration file
-    mass_ = params["quadrotor_dynamics"]["mass"].as<Scalar>();
-    arm_l_ = params["quadrotor_dynamics"]["arm_l"].as<Scalar>();
-    motor_omega_min_ =
-      params["quadrotor_dynamics"]["motor_omega_min"].as<Scalar>();
-    motor_omega_max_ =
-      params["quadrotor_dynamics"]["motor_omega_max"].as<Scalar>();
-    motor_tau_inv_ =
-      (1.0 / params["quadrotor_dynamics"]["motor_tau"].as<Scalar>());
-    std::vector<Scalar> thrust_map;
-    thrust_map =
-      params["quadrotor_dynamics"]["thrust_map"].as<std::vector<Scalar>>();
-    thrust_map_ = Map<Vector<3>>(thrust_map.data());
-    kappa_ = params["quadrotor_dynamics"]["kappa"].as<Scalar>();
-    std::vector<Scalar> omega_max;
-    omega_max =
-      params["quadrotor_dynamics"]["omega_max"].as<std::vector<Scalar>>();
-    omega_max_ = Map<Vector<3>>(omega_max.data());
-
-    // update relevant variables
-    updateInertiaMarix();
-    return valid();
-  } else {
-    return false;
-  }
-}
-
-bool QuadrotorDynamics::updateInertiaMarix() {
-  if (!valid()) return false;
-  t_BM_ = arm_l_ * sqrt(0.5) *
-          (Matrix<3, 4>() << 1, -1, -1, 1, -1, -1, 1, 1, 0, 0, 0, 0).finished();
-  J_ = mass_ / 12.0 * arm_l_ * arm_l_ * Vector<3>(4.5, 4.5, 7).asDiagonal();
-  J_inv_ = J_.inverse();
-  return true;
-}
-
-std::ostream& operator<<(std::ostream& os, const QuadrotorDynamics& quad) {
-  os.precision(3);
-  os << "Quadrotor Dynamics:\n"
-     << "mass =             [" << quad.mass_ << "]\n"
-     << "arm_l =            [" << quad.arm_l_ << "]\n"
-     << "t_BM =             [" << quad.t_BM_.row(0) << "]\n"
-     << "                   [" << quad.t_BM_.row(1) << "]\n"
-     << "                   [" << quad.t_BM_.row(2) << "]\n"
-     << "inertia =          [" << quad.J_.row(0) << "]\n"
-     << "                   [" << quad.J_.row(1) << "]\n"
-     << "                   [" << quad.J_.row(2) << "]\n"
-     << "motor_omega_min =  [" << quad.motor_omega_min_ << "]\n"
-     << "motor_omega_max =  [" << quad.motor_omega_max_ << "]\n"
-     << "motor_tau_inv =    [" << quad.motor_tau_inv_ << "]\n"
-     << "thrust_map =       [" << quad.thrust_map_.transpose() << "]\n"
-     << "kappa =            [" << quad.kappa_ << "]\n"
-     << "thrust_min =       [" << quad.thrust_min_ << "]\n"
-     << "thrust_max =       [" << quad.thrust_max_ << "]\n"
-     << "omega_max =        [" << quad.omega_max_.transpose() << "]"
-     << std::endl;
-  os.precision();
-  return os;
-}
-
-}  // namespace flightlib

flightlib/src/objects/object_base.cpp

@@ -1,9 +0,0 @@
-#include "flightlib/objects/object_base.hpp"
-
-namespace flightlib {
-
-ObjectBase::ObjectBase() {}
-
-ObjectBase::~ObjectBase() {}
-
-}  // namespace flightlib

flightlib/src/objects/quadrotor.cpp

@@ -2,19 +2,9 @@
 
 namespace flightlib {
 
-Quadrotor::Quadrotor(const std::string &cfg_path)
+Quadrotor::Quadrotor()
     : world_box_((Matrix<3, 2>() << -100, 100, -100, 100, -100, 100).finished()), size_(1.0, 1.0, 1.0), collision_(false) {
-	//
+	reset();
-	YAML::Node cfg = YAML::LoadFile(cfg_path);
-
-	// create quadrotor dynamics and update the parameters
-	dynamics_.updateParams(cfg);
-	init();
-}
-
-Quadrotor::Quadrotor(const QuadrotorDynamics &dynamics)
-    : world_box_((Matrix<3, 2>() << -100, 100, -100, 100, -100, 100).finished()), dynamics_(dynamics), size_(1.0, 1.0, 1.0), collision_(false) {
-	init();
 }
 
 Quadrotor::~Quadrotor() {}
@@ -30,69 +20,8 @@ bool Quadrotor::setState(const Ref<Vector<>> p_, const Ref<Vector<>> v_, const Q
 	return true;
 }
 
-bool Quadrotor::run(const Command &cmd, const Scalar ctl_dt) {
-	if (!setCommand(cmd))
-		return false;  // 限幅
-	return run(ctl_dt);
-}
-
-bool Quadrotor::run(const Scalar ctl_dt) {
-	if (!state_.valid())
-		return false;
-	if (!cmd_.valid())
-		return false;
-
-	QuadState old_state  = state_;
-	QuadState next_state = state_;
-
-	// time
-	const Scalar max_dt  = integrator_ptr_->dtMax();
-	Scalar remain_ctl_dt = ctl_dt;
-
-	// simulation loop
-	while (remain_ctl_dt > 0.0) {
-		const Scalar sim_dt = std::min(remain_ctl_dt, max_dt);
-
-		const Vector<4> motor_thrusts_des = cmd_.isSingleRotorThrusts() ? cmd_.thrusts : runFlightCtl(sim_dt, state_.w, cmd_);
-
-		runMotors(sim_dt, motor_thrusts_des);
-		// motor_thrusts_ = cmd_.thrusts;
-
-		const Vector<4> force_torques = B_allocation_ * motor_thrusts_;
-
-		// Compute linear acceleration and body torque
-		const Vector<3> force(0.0, 0.0, force_torques[0]);
-		state_.a = state_.q() * force * 1.0 / dynamics_.getMass() + gz_;
-
-		// compute body torque
-		state_.tau = force_torques.segment<3>(1);
-
-		// dynamics integration
-		integrator_ptr_->step(state_.x, sim_dt, next_state.x);
-
-		// update state and sim time
-		state_.qx /= state_.qx.norm();
-
-		//
-		state_.x = next_state.x;
-		remain_ctl_dt -= sim_dt;
-	}
-	state_.t += ctl_dt;
-	//
-	constrainInWorldBox(old_state);
-	return true;
-}
-
-void Quadrotor::init(void) {
-	// reset
-	updateDynamics(dynamics_);
-	reset();
-}
-
 bool Quadrotor::reset(void) {
 	state_.setZero();
-	motor_omega_.setZero();
-	motor_thrusts_.setZero();
 	return true;
 }
 
@@ -100,8 +29,6 @@ bool Quadrotor::reset(const QuadState &state) {
 	if (!state.valid())
 		return false;
 	state_ = state;
-	motor_omega_.setZero();
-	motor_thrusts_.setZero();
 	return true;
 }
 
@@ -147,51 +74,6 @@ void Quadrotor::runSimpleFlight(const Eigen::Vector3f &ref_acc, float ref_yaw, E
 	quat_des = Eigen::Quaternionf(R);
 }
 
-Vector<4> Quadrotor::runFlightCtl(const Scalar sim_dt, const Vector<3> &omega, const Command &command) {
-	const Scalar force = dynamics_.getMass() * command.collective_thrust;
-
-	const Vector<3> omega_err = command.omega - omega;
-
-	const Vector<3> body_torque_des = dynamics_.getJ() * Kinv_ang_vel_tau_ * omega_err + state_.w.cross(dynamics_.getJ() * state_.w);
-
-	const Vector<4> thrust_and_torque(force, body_torque_des.x(), body_torque_des.y(), body_torque_des.z());
-
-	const Vector<4> motor_thrusts_des = B_allocation_inv_ * thrust_and_torque;
-
-	return dynamics_.clampThrust(motor_thrusts_des);
-}
-
-void Quadrotor::runMotors(const Scalar sim_dt, const Vector<4> &motor_thruts_des) {
-	const Vector<4> motor_omega_des     = dynamics_.motorThrustToOmega(motor_thruts_des);
-	const Vector<4> motor_omega_clamped = dynamics_.clampMotorOmega(motor_omega_des);
-
-	// simulate motors as a first-order system
-	const Scalar c = std::exp(-sim_dt * dynamics_.getMotorTauInv());
-	motor_omega_   = c * motor_omega_ + (1.0 - c) * motor_omega_clamped;
-
-	motor_thrusts_ = dynamics_.motorOmegaToThrust(motor_omega_);
-	motor_thrusts_ = dynamics_.clampThrust(motor_thrusts_);
-}
-
-bool Quadrotor::setCommand(const Command &cmd) {
-	if (!cmd.valid())
-		return false;
-	cmd_ = cmd;
-
-	// 推力角速率
-	if (std::isfinite(cmd_.collective_thrust))
-		cmd_.collective_thrust = dynamics_.clampCollectiveThrust(cmd_.collective_thrust);
-
-	if (cmd_.omega.allFinite())
-		cmd_.omega = dynamics_.clampBodyrates(cmd_.omega);
-
-	// 转子推力
-	if (cmd_.thrusts.allFinite())
-		cmd_.thrusts = dynamics_.clampThrust(cmd_.thrusts);
-
-	return true;
-}
-
 bool Quadrotor::setState(const QuadState &state) {
 	if (!state.valid())
 		return false;
@@ -207,7 +89,6 @@ bool Quadrotor::setWorldBox(const Ref<Matrix<3, 2>> box) {
 	return true;
 }
 
-
 bool Quadrotor::constrainInWorldBox(const QuadState &old_state) {
 	if (!old_state.valid())
 		return false;
@@ -249,38 +130,6 @@ bool Quadrotor::getState(QuadState *const state) const {
 	return true;
 }
 
-bool Quadrotor::getMotorThrusts(Ref<Vector<4>> motor_thrusts) const {
-	motor_thrusts = motor_thrusts_;
-	return true;
-}
-
-bool Quadrotor::getMotorOmega(Ref<Vector<4>> motor_omega) const {
-	motor_omega = motor_omega_;
-	return true;
-}
-
-bool Quadrotor::getDynamics(QuadrotorDynamics *const dynamics) const {
-	if (!dynamics_.valid())
-		return false;
-	*dynamics = dynamics_;
-	return true;
-}
-
-const QuadrotorDynamics &Quadrotor::getDynamics() { return dynamics_; }
-
-bool Quadrotor::updateDynamics(const QuadrotorDynamics &dynamics) {
-	if (!dynamics.valid()) {
-		std::cout << "[Quadrotor] dynamics is not valid!" << std::endl;
-		return false;
-	}
-	dynamics_       = dynamics;
-	integrator_ptr_ = std::make_unique<IntegratorRK4>(dynamics_.getDynamicsFunction(), 2.5e-3);
-
-	B_allocation_     = dynamics_.getAllocationMatrix();
-	B_allocation_inv_ = B_allocation_.inverse();
-	return true;
-}
-
 bool Quadrotor::addRGBCamera(std::shared_ptr<RGBCamera> camera) {
 	rgb_cameras_.push_back(camera);
 	return true;