hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Simplify the code and remove drone dynamics (unused)

Browse Source
This commit is contained in:
Lu Junjie
2024-10-22 11:13:26 +08:00
parent 004ff2fea9
commit e9aa4e406d
19 changed files with 13 additions and 794 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
flightlib/include/flightlib/common/command.hpp
View File

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

32
flightlib/include/flightlib/common/integrator_base.hpp
View File

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

17
flightlib/include/flightlib/common/integrator_euler.hpp
View File

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

17
flightlib/include/flightlib/common/integrator_rk4.hpp
View File

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

21
flightlib/include/flightlib/dynamics/dynamics_base.hpp
View File

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

87
flightlib/include/flightlib/dynamics/quadrotor_dynamics.hpp
View File

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

1
flightlib/include/flightlib/envs/quadrotor_env.hpp
View File

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

19
flightlib/include/flightlib/objects/object_base.hpp
View File

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

61
flightlib/include/flightlib/objects/quadrotor.hpp
View File

@@ -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/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<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);
// 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<Vector<3>> size) { size_ = size; };
inline void setCollision(const bool collision) { collision_ = collision; };
// 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
QuadrotorDynamics dynamics_;
// quadrotor sensors
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_;
};