Simplify the code and remove drone dynamics (unused)
This commit is contained in:
Lu Junjie
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
@@ -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_;
|
||||
};
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -1,9 +0,0 @@
|
||||
#include "flightlib/dynamics/dynamics_base.hpp"
|
||||
|
||||
namespace flightlib {
|
||||
|
||||
DynamicsBase::DynamicsBase() {}
|
||||
|
||||
DynamicsBase::~DynamicsBase() {}
|
||||
|
||||
} // namespace flightlib
|
||||
@@ -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
|
||||
@@ -1,9 +0,0 @@
|
||||
#include "flightlib/objects/object_base.hpp"
|
||||
|
||||
namespace flightlib {
|
||||
|
||||
ObjectBase::ObjectBase() {}
|
||||
|
||||
ObjectBase::~ObjectBase() {}
|
||||
|
||||
} // namespace flightlib
|
||||
@@ -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<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;
|
||||
|
||||
Reference in New Issue
Block a user