Simplify the code and remove drone dynamics (unused)
This commit is contained in:
Lu Junjie
@@ -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