From e9aa4e406dfeb58daff028367b627d6b663d259b Mon Sep 17 00:00:00 2001 From: Lu Junjie Date: Tue, 22 Oct 2024 11:13:26 +0800 Subject: [PATCH] Simplify the code and remove drone dynamics (unused) --- README.md | 2 +- flightlib/CMakeLists.txt | 1 - .../include/flightlib/common/command.hpp | 37 --- .../flightlib/common/integrator_base.hpp | 32 --- .../flightlib/common/integrator_euler.hpp | 17 -- .../flightlib/common/integrator_rk4.hpp | 17 -- .../flightlib/dynamics/dynamics_base.hpp | 21 -- .../flightlib/dynamics/quadrotor_dynamics.hpp | 87 ------- .../include/flightlib/envs/quadrotor_env.hpp | 1 - .../include/flightlib/objects/object_base.hpp | 19 -- .../include/flightlib/objects/quadrotor.hpp | 61 +---- flightlib/src/common/command.cpp | 29 --- flightlib/src/common/integrator_base.cpp | 33 --- flightlib/src/common/integrator_euler.cpp | 15 -- flightlib/src/common/integrator_rk4.cpp | 34 --- flightlib/src/dynamics/dynamics_base.cpp | 9 - flightlib/src/dynamics/quadrotor_dynamics.cpp | 228 ------------------ flightlib/src/objects/object_base.cpp | 9 - flightlib/src/objects/quadrotor.cpp | 155 +----------- 19 files changed, 13 insertions(+), 794 deletions(-) delete mode 100644 flightlib/include/flightlib/common/command.hpp delete mode 100644 flightlib/include/flightlib/common/integrator_base.hpp delete mode 100644 flightlib/include/flightlib/common/integrator_euler.hpp delete mode 100644 flightlib/include/flightlib/common/integrator_rk4.hpp delete mode 100644 flightlib/include/flightlib/dynamics/dynamics_base.hpp delete mode 100644 flightlib/include/flightlib/dynamics/quadrotor_dynamics.hpp delete mode 100644 flightlib/include/flightlib/objects/object_base.hpp delete mode 100644 flightlib/src/common/command.cpp delete mode 100644 flightlib/src/common/integrator_base.cpp delete mode 100644 flightlib/src/common/integrator_euler.cpp delete mode 100644 flightlib/src/common/integrator_rk4.cpp delete mode 100644 flightlib/src/dynamics/dynamics_base.cpp delete mode 100644 flightlib/src/dynamics/quadrotor_dynamics.cpp delete mode 100644 flightlib/src/objects/object_base.cpp diff --git a/README.md b/README.md index 8169ecf..4c0706a 100644 --- a/README.md +++ b/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 diff --git a/flightlib/CMakeLists.txt b/flightlib/CMakeLists.txt index 6a7a0a5..213d25d 100644 --- a/flightlib/CMakeLists.txt +++ b/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 diff --git a/flightlib/include/flightlib/common/command.hpp b/flightlib/include/flightlib/common/command.hpp deleted file mode 100644 index b00df8e..0000000 --- a/flightlib/include/flightlib/common/command.hpp +++ /dev/null @@ -1,37 +0,0 @@ - -#pragma once - -#include - -#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 \ No newline at end of file diff --git a/flightlib/include/flightlib/common/integrator_base.hpp b/flightlib/include/flightlib/common/integrator_base.hpp deleted file mode 100644 index 1d1e518..0000000 --- a/flightlib/include/flightlib/common/integrator_base.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include - -#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>, Ref>)>; - IntegratorBase(DynamicsFunction function, const Scalar dt_max = 1e-3); - - bool integrate(const QuadState& initial, QuadState* const final) const; - - bool integrate(const Ref> initial, const Scalar dt, - Ref> final) const; - - virtual bool step(const Ref> initial, const Scalar dt, - Ref> final) const = 0; - - Scalar dtMax() const; - - protected: - DynamicsFunction dynamics_; - Scalar dt_max_; -}; - -} // namespace flightlib \ No newline at end of file diff --git a/flightlib/include/flightlib/common/integrator_euler.hpp b/flightlib/include/flightlib/common/integrator_euler.hpp deleted file mode 100644 index 42b8923..0000000 --- a/flightlib/include/flightlib/common/integrator_euler.hpp +++ /dev/null @@ -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> initial, const Scalar dt, - Ref> final) const; -}; - -} // namespace flightlib \ No newline at end of file diff --git a/flightlib/include/flightlib/common/integrator_rk4.hpp b/flightlib/include/flightlib/common/integrator_rk4.hpp deleted file mode 100644 index 2b95d24..0000000 --- a/flightlib/include/flightlib/common/integrator_rk4.hpp +++ /dev/null @@ -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> initial, const Scalar dt, - Ref> final) const; -}; - -} // namespace flightlib \ No newline at end of file diff --git a/flightlib/include/flightlib/dynamics/dynamics_base.hpp b/flightlib/include/flightlib/dynamics/dynamics_base.hpp deleted file mode 100644 index 468db07..0000000 --- a/flightlib/include/flightlib/dynamics/dynamics_base.hpp +++ /dev/null @@ -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>, Ref>)>; - - DynamicsBase(); - virtual ~DynamicsBase(); - - // public get function - virtual DynamicsFunction getDynamicsFunction() const = 0; - - private: -}; - -} // namespace flightlib diff --git a/flightlib/include/flightlib/dynamics/quadrotor_dynamics.hpp b/flightlib/include/flightlib/dynamics/quadrotor_dynamics.hpp deleted file mode 100644 index 26ffb98..0000000 --- a/flightlib/include/flightlib/dynamics/quadrotor_dynamics.hpp +++ /dev/null @@ -1,87 +0,0 @@ -#pragma once - -#include - -#include -#include - -#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> state, Ref> 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 diff --git a/flightlib/include/flightlib/envs/quadrotor_env.hpp b/flightlib/include/flightlib/envs/quadrotor_env.hpp index 76cb5f4..6bdb1a0 100755 --- a/flightlib/include/flightlib/envs/quadrotor_env.hpp +++ b/flightlib/include/flightlib/envs/quadrotor_env.hpp @@ -12,7 +12,6 @@ #include // 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" diff --git a/flightlib/include/flightlib/objects/object_base.hpp b/flightlib/include/flightlib/objects/object_base.hpp deleted file mode 100644 index b826952..0000000 --- a/flightlib/include/flightlib/objects/object_base.hpp +++ /dev/null @@ -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 diff --git a/flightlib/include/flightlib/objects/quadrotor.hpp b/flightlib/include/flightlib/objects/quadrotor.hpp index 9951810..4110922 100644 --- a/flightlib/include/flightlib/objects/quadrotor.hpp +++ b/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 -#include "flightlib/common/command.hpp" -#include "flightlib/common/integrator_rk4.hpp" #include "flightlib/common/types.hpp" -#include "flightlib/dynamics/quadrotor_dynamics.hpp" -#include "flightlib/objects/object_base.hpp" +#include "flightlib/common/quad_state.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(const QuadrotorDynamics& dynamics = QuadrotorDynamics(1.0, 0.25)); + Quadrotor(); ~Quadrotor(); // reset - bool reset(void) override; + bool reset(void); bool reset(const QuadState& state); - void init(void); // run the quadrotor bool setState(const Ref> p, const Ref> v, const Quaternion q_, const Ref> 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> motor_thrusts) const; - bool getMotorOmega(Ref> 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> getCameras(void) const; - bool getCamera(const size_t cam_id, std::shared_ptr camera) const; int getNumCamera() const; bool getCollision() const; + std::vector> getCameras(void) const; + bool getCamera(const size_t cam_id, std::shared_ptr 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 camera); - - // low-level controller - 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); + inline void setSize(const Ref> size) { size_ = size; }; + inline void setCollision(const bool collision) { collision_ = collision; }; // constrain world box bool setWorldBox(const Ref> box); bool constrainInWorldBox(const QuadState& old_state); - // - inline Scalar getMass(void) { return dynamics_.getMass(); }; - inline void setSize(const Ref> size) { size_ = size; }; - inline void setCollision(const bool collision) { collision_ = collision; }; - - float getSimT() { return state_.t; } - private: - // quadrotor dynamics, integrators - QuadrotorDynamics dynamics_; + // quadrotor sensors IMU imu_; - std::unique_ptr integrator_ptr_; std::vector> 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_; }; diff --git a/flightlib/src/common/command.cpp b/flightlib/src/common/command.cpp deleted file mode 100644 index de76741..0000000 --- a/flightlib/src/common/command.cpp +++ /dev/null @@ -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 \ No newline at end of file diff --git a/flightlib/src/common/integrator_base.cpp b/flightlib/src/common/integrator_base.cpp deleted file mode 100644 index 7827c93..0000000 --- a/flightlib/src/common/integrator_base.cpp +++ /dev/null @@ -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> initial, - const Scalar dt, Ref> 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 \ No newline at end of file diff --git a/flightlib/src/common/integrator_euler.cpp b/flightlib/src/common/integrator_euler.cpp deleted file mode 100644 index c53f308..0000000 --- a/flightlib/src/common/integrator_euler.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "flightlib/common/integrator_euler.hpp" - -namespace flightlib { - -bool IntegratorEuler::step(const Ref> initial, const Scalar dt, - Ref> final) const { - Vector<> derivative(initial.rows()); - if (!this->dynamics_(initial, derivative)) return false; - - final = initial + dt * derivative; - - return true; -} - -} // namespace flightlib \ No newline at end of file diff --git a/flightlib/src/common/integrator_rk4.cpp b/flightlib/src/common/integrator_rk4.cpp deleted file mode 100644 index 05b9d4d..0000000 --- a/flightlib/src/common/integrator_rk4.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "flightlib/common/integrator_rk4.hpp" - -namespace flightlib { - -bool IntegratorRK4::step(const Ref> initial, const Scalar dt, - Ref> 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 \ No newline at end of file diff --git a/flightlib/src/dynamics/dynamics_base.cpp b/flightlib/src/dynamics/dynamics_base.cpp deleted file mode 100644 index ab393b0..0000000 --- a/flightlib/src/dynamics/dynamics_base.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "flightlib/dynamics/dynamics_base.hpp" - -namespace flightlib { - -DynamicsBase::DynamicsBase() {} - -DynamicsBase::~DynamicsBase() {} - -} // namespace flightlib diff --git a/flightlib/src/dynamics/quadrotor_dynamics.cpp b/flightlib/src/dynamics/quadrotor_dynamics.cpp deleted file mode 100644 index 00e5ee3..0000000 --- a/flightlib/src/dynamics/quadrotor_dynamics.cpp +++ /dev/null @@ -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> state, - Ref> dstate) const { - if (!state.segment(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::TAU); - - // linear velocity = dx / dt - dstate.segment(QS::POS) = state.segment(QS::VEL); - - // differentiate quaternion = dq / dt - dstate.segment(QS::ATT) = - 0.5 * Q_right(q_omega) * state.segment(QS::ATT); - - // linear acceleration = dv / dt - dstate.segment(QS::VEL) = state.segment(QS::ACC); - - // angular accleration = domega / dt - dstate.segment(QS::OME) = - J_inv_ * (body_torque - omega.cross(J_ * omega)); - // - return true; -} - -QuadrotorDynamics::DynamicsFunction QuadrotorDynamics::getDynamicsFunction() - const { - return std::bind( - static_cast>, - Ref>) 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(); - arm_l_ = params["quadrotor_dynamics"]["arm_l"].as(); - motor_omega_min_ = - params["quadrotor_dynamics"]["motor_omega_min"].as(); - motor_omega_max_ = - params["quadrotor_dynamics"]["motor_omega_max"].as(); - motor_tau_inv_ = - (1.0 / params["quadrotor_dynamics"]["motor_tau"].as()); - std::vector thrust_map; - thrust_map = - params["quadrotor_dynamics"]["thrust_map"].as>(); - thrust_map_ = Map>(thrust_map.data()); - kappa_ = params["quadrotor_dynamics"]["kappa"].as(); - std::vector omega_max; - omega_max = - params["quadrotor_dynamics"]["omega_max"].as>(); - omega_max_ = Map>(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 diff --git a/flightlib/src/objects/object_base.cpp b/flightlib/src/objects/object_base.cpp deleted file mode 100644 index 97213f4..0000000 --- a/flightlib/src/objects/object_base.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "flightlib/objects/object_base.hpp" - -namespace flightlib { - -ObjectBase::ObjectBase() {} - -ObjectBase::~ObjectBase() {} - -} // namespace flightlib diff --git a/flightlib/src/objects/quadrotor.cpp b/flightlib/src/objects/quadrotor.cpp index df5542a..231657f 100644 --- a/flightlib/src/objects/quadrotor.cpp +++ b/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) { - // - 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(); + reset(); } Quadrotor::~Quadrotor() {} @@ -30,69 +20,8 @@ bool Quadrotor::setState(const Ref> p_, const Ref> 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> 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> motor_thrusts) const { - motor_thrusts = motor_thrusts_; - return true; -} - -bool Quadrotor::getMotorOmega(Ref> 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(dynamics_.getDynamicsFunction(), 2.5e-3); - - B_allocation_ = dynamics_.getAllocationMatrix(); - B_allocation_inv_ = B_allocation_.inverse(); - return true; -} - bool Quadrotor::addRGBCamera(std::shared_ptr camera) { rgb_cameras_.push_back(camera); return true;