зеркало из https://github.com/microsoft/AirSim.git
require reset() as first call, checks for reset(), update() seq
This commit is contained in:
Shital Shah
Родитель
039797961a
Коммит
d7cd82bd74
|
|
@ -6,6 +6,7 @@
|
|||
|
||||
#include "ClockBase.hpp"
|
||||
#include "Common.hpp"
|
||||
#include <atomic>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
|
@ -52,7 +53,7 @@ public:
|
|||
|
||||
|
||||
private:
|
||||
TTimePoint current_;
|
||||
std::atomic<TTimePoint> current_;
|
||||
TTimeDelta step_;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@ public:
|
|||
void initialize(TTimeDelta delay) //in seconds
|
||||
{
|
||||
setDelay(delay);
|
||||
DelayLine::reset();
|
||||
}
|
||||
void setDelay(TTimeDelta delay)
|
||||
{
|
||||
|
|
@ -36,6 +35,8 @@ public:
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
values_.clear();
|
||||
times_.clear();
|
||||
last_time_ = 0;
|
||||
|
|
@ -44,6 +45,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
if (!times_.empty() &&
|
||||
ClockBase::elapsedBetween(clock()->nowNanos(), times_.front()) >= delay_) {
|
||||
|
||||
|
|
|
|||
|
|
@ -142,6 +142,9 @@ public:
|
|||
//Below 51km: Practical Meteorology by Roland Stull, pg 12
|
||||
//Above 51km: http://www.braeunig.us/space/atmmodel.htm
|
||||
//Validation data: https://www.avs.org/AVS/files/c7/c7edaedb-95b2-438f-adfb-36de54f87b9e.pdf
|
||||
|
||||
//TODO: handle -ve altitude better (shouldn't grow indefinitely!)
|
||||
|
||||
if (geopot_height <= 11)
|
||||
//at alt 0, return sea level pressure
|
||||
return SeaLevelPressure * powf(288.15f / std_temperature, -5.255877f);
|
||||
|
|
|
|||
|
|
@ -39,12 +39,13 @@ public:
|
|||
timeConstant_ = timeConstant;
|
||||
initial_input_ = initial_input;
|
||||
initial_output_ = initial_output;
|
||||
FirstOrderFilter::reset();
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
last_time_ = clock()->nowNanos();
|
||||
input_ = initial_input_;
|
||||
output_ = initial_output_;
|
||||
|
|
@ -52,6 +53,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
|
||||
//lower the weight for previous value if its been long time
|
||||
|
|
|
|||
|
|
@ -21,13 +21,13 @@ public:
|
|||
{
|
||||
frequency_ = frequency;
|
||||
startup_delay_ = startup_delay;
|
||||
reset();
|
||||
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
last_time_ = clock()->nowNanos();
|
||||
first_time_ = last_time_;
|
||||
|
||||
|
|
@ -46,6 +46,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
elapsed_total_sec_ = clock()->elapsedSince(first_time_);
|
||||
elapsed_interval_sec_ = clock()->elapsedSince(last_time_);
|
||||
++update_count_;
|
||||
|
|
|
|||
|
|
@ -29,13 +29,13 @@ public:
|
|||
initial_output_ = getNextRandom() * sigma_;
|
||||
else
|
||||
initial_output_ = initial_output;
|
||||
|
||||
GaussianMarkov::reset();
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
last_time_ = clock()->nowNanos();
|
||||
output_ = initial_output_;
|
||||
rand_.reset();
|
||||
|
|
@ -52,6 +52,8 @@ public:
|
|||
John H Wall, 2007, eq 2.5, pg 13, http://etd.auburn.edu/handle/10415/945
|
||||
*/
|
||||
|
||||
UpdatableObject::update();
|
||||
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
|
||||
double alpha = exp(-dt / tau_);
|
||||
|
|
|
|||
|
|
@ -28,7 +28,6 @@ public:
|
|||
enabled_ = enabled;
|
||||
report_.initialize(float_precision, is_scientific_notation);
|
||||
report_freq_.initialize(DefaultReportFreq);
|
||||
StateReporterWrapper::reset();
|
||||
}
|
||||
|
||||
void clearReport()
|
||||
|
|
@ -40,6 +39,8 @@ public:
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
last_time_ = clock()->nowNanos();
|
||||
clearReport();
|
||||
dt_stats_.clear();
|
||||
|
|
@ -48,6 +49,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
|
||||
if (enabled_) {
|
||||
|
|
|
|||
|
|
@ -33,12 +33,16 @@ public:
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
for (TUpdatableObjectPtr& member : members_)
|
||||
member->reset();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
for (TUpdatableObjectPtr& member : members_)
|
||||
member->update();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,8 +12,20 @@ namespace msr { namespace airlib {
|
|||
|
||||
class UpdatableObject {
|
||||
public:
|
||||
virtual void reset() = 0;
|
||||
virtual void update() = 0;
|
||||
virtual void reset()
|
||||
{
|
||||
if (reset_called && !update_called)
|
||||
throw std::runtime_error("Multiple reset() calls detected without call to update()");
|
||||
|
||||
reset_called = true;
|
||||
}
|
||||
virtual void update()
|
||||
{
|
||||
if (!reset_called)
|
||||
throw std::runtime_error("reset() must be called first before update()");
|
||||
update_called = true;
|
||||
}
|
||||
|
||||
virtual ~UpdatableObject() = default;
|
||||
|
||||
virtual void reportState(StateReporter& reporter)
|
||||
|
|
@ -31,6 +43,10 @@ public:
|
|||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
|
||||
private:
|
||||
bool reset_called = false;
|
||||
bool update_called = false;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
namespace msr { namespace airlib {
|
||||
|
||||
/*
|
||||
This class defined the interface for vehicle controllers. A typical vehicle controller would consume
|
||||
This class defines the interface for vehicle controllers. A typical vehicle controller would consume
|
||||
sensor values as inputs and return control signals for various vertices on physics body as output. The update()
|
||||
method should be used to process the sensor inputs from vehicle as needed (the implementor should have vehicle object
|
||||
possibly through initialization). The getVertexControlSignal() is called to retrieve the value of control signal to be applied
|
||||
|
|
@ -20,10 +20,6 @@ namespace msr { namespace airlib {
|
|||
*/
|
||||
class ControllerBase : public UpdatableObject {
|
||||
public:
|
||||
//reset any state in the controller
|
||||
virtual void reset() override = 0;
|
||||
virtual void update() override = 0;
|
||||
|
||||
//return 0 to 1 (corresponds to zero to full thrust)
|
||||
virtual real_T getVertexControlSignal(unsigned int rotor_index) = 0;
|
||||
virtual size_t getVertexCount() = 0;
|
||||
|
|
|
|||
|
|
@ -1297,10 +1297,12 @@ std::string MavLinkDroneController::findPixhawk()
|
|||
//*** Start: VehicleControllerBase implementation ***//
|
||||
void MavLinkDroneController::reset()
|
||||
{
|
||||
DroneControllerBase::reset();
|
||||
pimpl_->reset();
|
||||
}
|
||||
void MavLinkDroneController::update()
|
||||
{
|
||||
DroneControllerBase::update();
|
||||
pimpl_->update();
|
||||
}
|
||||
real_T MavLinkDroneController::getVertexControlSignal(unsigned int rotor_index)
|
||||
|
|
|
|||
|
|
@ -25,20 +25,23 @@ public:
|
|||
{
|
||||
params_ = params;
|
||||
motor_control_signals_.resize(params_.rotor_count);
|
||||
MotorDirectController::reset();
|
||||
}
|
||||
|
||||
|
||||
//*** Start ControllerBase implementation ****//
|
||||
virtual void reset() override
|
||||
{
|
||||
ControllerBase::reset();
|
||||
|
||||
motor_control_signals_.assign(params_.rotor_count, 0);
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
//nothing to update in direct motor control
|
||||
}
|
||||
virtual void update() override
|
||||
{
|
||||
ControllerBase::update();
|
||||
|
||||
//nothing to update in direct motor control
|
||||
}
|
||||
|
||||
virtual real_T getVertexControlSignal(unsigned int rotor_index) override
|
||||
{
|
||||
|
|
|
|||
|
|
@ -24,27 +24,30 @@ public:
|
|||
|
||||
if (params_.rotor_count != 4)
|
||||
throw std::invalid_argument(Utils::stringf("Rotor count of %d is not supported yet", params_.rotor_count));
|
||||
RpyDirectController::reset();
|
||||
}
|
||||
|
||||
//*** Start ControllerBase implementation ****//
|
||||
virtual void reset() override
|
||||
{
|
||||
motor_control_signals_.assign(params_.rotor_count, 0);
|
||||
}
|
||||
virtual void reset() override
|
||||
{
|
||||
ControllerBase::reset();
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
real_T throttle_speed = scale(throttle_, 1.0f / params_.throttle_scale);
|
||||
real_T roll_speed = scale(roll_, throttle_speed / params_.roll_scale);
|
||||
real_T pitch_speed = scale(pitch_, throttle_speed / params_.pitch_scale);
|
||||
real_T yaw_speed = scale(yaw_, throttle_speed / params_.yaw_scale);
|
||||
motor_control_signals_.assign(params_.rotor_count, 0);
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
ControllerBase::update();
|
||||
|
||||
real_T throttle_speed = scale(throttle_, 1.0f / params_.throttle_scale);
|
||||
real_T roll_speed = scale(roll_, throttle_speed / params_.roll_scale);
|
||||
real_T pitch_speed = scale(pitch_, throttle_speed / params_.pitch_scale);
|
||||
real_T yaw_speed = scale(yaw_, throttle_speed / params_.yaw_scale);
|
||||
|
||||
motor_control_signals_[0] = Utils::clip(throttle_speed - pitch_speed + roll_speed + yaw_speed, 0.0f, 1.0f);
|
||||
motor_control_signals_[1] = Utils::clip(throttle_speed + pitch_speed + roll_speed - yaw_speed, 0.0f, 1.0f);
|
||||
motor_control_signals_[2] = Utils::clip(throttle_speed + pitch_speed - roll_speed + yaw_speed, 0.0f, 1.0f);
|
||||
motor_control_signals_[3] = Utils::clip(throttle_speed - pitch_speed - roll_speed - yaw_speed, 0.0f, 1.0f);
|
||||
}
|
||||
}
|
||||
|
||||
virtual real_T getVertexControlSignal(unsigned int rotor_index) override
|
||||
{
|
||||
|
|
@ -66,11 +69,11 @@ public:
|
|||
virtual ~RpyDirectController() = default;
|
||||
|
||||
private:
|
||||
//input between -1 to 1
|
||||
real_T scale(real_T input, real_T factor)
|
||||
{
|
||||
return input * factor;
|
||||
}
|
||||
//input between -1 to 1
|
||||
real_T scale(real_T input, real_T factor)
|
||||
{
|
||||
return input * factor;
|
||||
}
|
||||
|
||||
private:
|
||||
real_T roll_ = 0, pitch_ = 0, yaw_ = 0, throttle_ = 0;
|
||||
|
|
|
|||
|
|
@ -48,20 +48,26 @@ public:
|
|||
//*** Start: VehicleControllerBase implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
DroneControllerBase::reset();
|
||||
|
||||
board_->system_reset(false);
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
DroneControllerBase::update();
|
||||
|
||||
board_->notifySensorUpdated(ros_flight::Board::SensorType::Imu);
|
||||
firmware_->loop();
|
||||
}
|
||||
|
||||
virtual void start() override
|
||||
{
|
||||
DroneControllerBase::start();
|
||||
}
|
||||
virtual void stop() override
|
||||
{
|
||||
DroneControllerBase::stop();
|
||||
}
|
||||
|
||||
virtual size_t getVertexCount() override
|
||||
|
|
|
|||
|
|
@ -90,12 +90,16 @@ public:
|
|||
|
||||
virtual void reset() override
|
||||
{
|
||||
IBoard::reset();
|
||||
|
||||
motor_output_.assign(params_->motor_count, 0);
|
||||
input_channels_.assign(params_->rc_channel_count, 0);
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
IBoard::update();
|
||||
|
||||
//no op for now
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -25,11 +25,14 @@ public: // derived class specific methods
|
|||
public: // implement CommLink interface
|
||||
virtual void reset()
|
||||
{
|
||||
simple_flight::ICommLink::reset();
|
||||
|
||||
messages_.clear();
|
||||
}
|
||||
|
||||
virtual void update()
|
||||
{
|
||||
simple_flight::ICommLink::update();
|
||||
}
|
||||
|
||||
virtual void log(const std::string& message, int32_t log_level)
|
||||
|
|
|
|||
|
|
@ -32,7 +32,6 @@ public:
|
|||
|
||||
//create firmware
|
||||
firmware_.reset(new simple_flight::Firmware(board_.get(), comm_link_.get(), estimator_.get(), ¶ms_));
|
||||
firmware_->reset();
|
||||
|
||||
//find out which RC we should use
|
||||
Settings child;
|
||||
|
|
@ -52,20 +51,26 @@ public:
|
|||
//*** Start: VehicleControllerBase implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
DroneControllerBase::reset();
|
||||
|
||||
firmware_->reset();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
DroneControllerBase::update();
|
||||
|
||||
firmware_->update();
|
||||
}
|
||||
|
||||
virtual void start() override
|
||||
{
|
||||
DroneControllerBase::start();
|
||||
}
|
||||
|
||||
virtual void stop() override
|
||||
{
|
||||
DroneControllerBase::stop();
|
||||
}
|
||||
|
||||
virtual size_t getVertexCount() override
|
||||
|
|
|
|||
|
|
@ -44,6 +44,9 @@ public:
|
|||
|
||||
virtual void reset() override
|
||||
{
|
||||
IAxisController::reset();
|
||||
IGoalInput::reset();
|
||||
|
||||
pid_->reset();
|
||||
rate_controller_->reset();
|
||||
rate_goal_ = Axis4r();
|
||||
|
|
@ -52,6 +55,9 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
IAxisController::update();
|
||||
IGoalInput::update();
|
||||
|
||||
//get response of level PID
|
||||
const auto& level_goal = goal_input_->getGoal();
|
||||
pid_->setGoal(level_goal.axis3[axis_]);
|
||||
|
|
|
|||
|
|
@ -30,12 +30,16 @@ public:
|
|||
|
||||
virtual void reset() override
|
||||
{
|
||||
IAxisController::reset();
|
||||
|
||||
pid_->reset();
|
||||
output_ = TReal();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
IAxisController::update();
|
||||
|
||||
pid_->setGoal(goal_input_->getGoal().axis3[axis_]);
|
||||
pid_->setMeasured(state_estimator_->getAngulerVelocity()[axis_]);
|
||||
pid_->update();
|
||||
|
|
|
|||
|
|
@ -27,6 +27,8 @@ public:
|
|||
|
||||
virtual void reset() override
|
||||
{
|
||||
IController::reset();
|
||||
|
||||
output_ = Axis4r();
|
||||
|
||||
for (unsigned int axis = 0; axis < 3; ++axis) {
|
||||
|
|
@ -38,6 +40,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
IController::update();
|
||||
|
||||
const auto& goal_mode = goal_input_->getGoalMode();
|
||||
|
||||
//for now we set throttle to same as goal
|
||||
|
|
@ -60,6 +64,7 @@ public:
|
|||
|
||||
//initialize axis controller
|
||||
axis_controllers_[axis]->initialize(axis, goal_input_, state_estimator_);
|
||||
axis_controllers_[axis]->reset();
|
||||
}
|
||||
|
||||
//update axis controller
|
||||
|
|
|
|||
|
|
@ -80,6 +80,8 @@ public:
|
|||
|
||||
virtual void reset() override
|
||||
{
|
||||
IUpdatable::reset();
|
||||
|
||||
goal_ = T();
|
||||
measured_ = T();
|
||||
last_time_ = clock_ == nullptr ? 0 : clock_->millis();
|
||||
|
|
@ -90,6 +92,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
IUpdatable::update();
|
||||
|
||||
if (!config_.enabled)
|
||||
return;
|
||||
|
||||
|
|
|
|||
|
|
@ -18,6 +18,8 @@ public:
|
|||
|
||||
virtual void reset() override
|
||||
{
|
||||
IGoalInput::reset();
|
||||
|
||||
rc_channels_.assign(params_->rc_channel_count, 0);
|
||||
last_rec_read_ = 0;
|
||||
|
||||
|
|
@ -27,6 +29,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
IGoalInput::update();
|
||||
|
||||
uint64_t time = clock_->millis();
|
||||
|
||||
//don't keep reading if not updated
|
||||
|
|
|
|||
|
|
@ -11,8 +11,6 @@ namespace simple_flight {
|
|||
class IAxisController : public IUpdatable {
|
||||
public:
|
||||
virtual void initialize(unsigned int axis, const IGoalInput* goal_input, const IStateEstimator* state_estimator) = 0;
|
||||
virtual void reset() override = 0;
|
||||
virtual void update() override = 0;
|
||||
virtual TReal getOutput() = 0;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -11,8 +11,6 @@ namespace simple_flight {
|
|||
class IController : public IUpdatable {
|
||||
public:
|
||||
virtual void initialize(const IGoalInput* goal_input, const IStateEstimator* state_estimator) = 0;
|
||||
virtual void reset() override = 0;
|
||||
virtual void update() override = 0;
|
||||
virtual const Axis4r& getOutput() = 0;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -4,8 +4,26 @@ namespace simple_flight {
|
|||
|
||||
class IUpdatable {
|
||||
public:
|
||||
virtual void reset() = 0;
|
||||
virtual void update() = 0;
|
||||
virtual void reset()
|
||||
{
|
||||
if (reset_called && !update_called)
|
||||
throw std::runtime_error("Multiple reset() calls detected without call to update()");
|
||||
|
||||
reset_called = true;
|
||||
}
|
||||
virtual void update()
|
||||
{
|
||||
if (!reset_called)
|
||||
throw std::runtime_error("reset() must be called first before update()");
|
||||
update_called = true;
|
||||
}
|
||||
|
||||
virtual ~IUpdatable() = default;
|
||||
|
||||
|
||||
private:
|
||||
bool reset_called = false;
|
||||
bool update_called = false;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -48,8 +48,6 @@ public:
|
|||
home_geo_point_ = EarthUtils::HomeGeoPoint(initial_.geo_point);
|
||||
|
||||
updateState(initial_, home_geo_point_);
|
||||
|
||||
Environment::reset();
|
||||
}
|
||||
|
||||
//in local NED coordinates
|
||||
|
|
|
|||
|
|
@ -18,13 +18,14 @@ public:
|
|||
FastPhysicsEngine(bool enable_ground_lock = true)
|
||||
: enable_ground_lock_(enable_ground_lock)
|
||||
{
|
||||
FastPhysicsEngine::reset();
|
||||
}
|
||||
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
PhysicsEngineBase::reset();
|
||||
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
initPhysicsBody(body_ptr);
|
||||
}
|
||||
|
|
@ -39,6 +40,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
PhysicsEngineBase::update();
|
||||
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
updatePhysics(*body_ptr);
|
||||
}
|
||||
|
|
@ -74,7 +77,7 @@ private:
|
|||
//first compute the response as if there was no collision
|
||||
//this is necessory to take in to account forces and torques generated by body
|
||||
getNextKinematicsNoCollison(dt, body, current, next, next_wrench);
|
||||
|
||||
|
||||
//if there is collison, see if we need collison response
|
||||
const CollisionInfo collison_info = body.getCollisionInfo();
|
||||
CollisonResponseInfo& collison_response_info = body.getCollisionResponseInfo();
|
||||
|
|
@ -193,7 +196,6 @@ private:
|
|||
//else keep the orientation
|
||||
next.pose.position = collison_info.position + (collison_info.normal * collison_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles);
|
||||
|
||||
|
||||
next_wrench = Wrench::zero();
|
||||
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -36,17 +36,20 @@ public:
|
|||
void initialize(const State& initial)
|
||||
{
|
||||
initial_ = initial;
|
||||
Kinematics::reset();
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
current_ = initial_;
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
//nothing to do because next state should be updated
|
||||
//by physics engine. The reason is that final state
|
||||
//needs to take in to account state of other objects as well,
|
||||
|
|
|
|||
|
|
@ -74,8 +74,6 @@ public: //methods
|
|||
inertia_ = inertia;
|
||||
inertia_inv_ = inertia_.inverse();
|
||||
environment_ = environment;
|
||||
|
||||
PhysicsBody::reset();
|
||||
}
|
||||
|
||||
//enable physics body detection
|
||||
|
|
@ -88,6 +86,8 @@ public: //methods
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
kinematics_.reset();
|
||||
|
||||
if (environment_)
|
||||
|
|
@ -95,10 +95,20 @@ public: //methods
|
|||
wrench_ = Wrench::zero();
|
||||
collison_info_ = CollisionInfo();
|
||||
collison_response_info_ = CollisonResponseInfo();
|
||||
|
||||
//update individual vertices
|
||||
for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) {
|
||||
getWrenchVertex(vertex_index).reset();
|
||||
}
|
||||
for (uint vertex_index = 0; vertex_index < dragVertexCount(); ++vertex_index) {
|
||||
getDragVertex(vertex_index).reset();
|
||||
}
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
//update position from kinematics so we have latest position after physics update
|
||||
environment_->setPosition(getKinematics().pose.position);
|
||||
environment_->update();
|
||||
|
|
@ -108,6 +118,9 @@ public: //methods
|
|||
//update individual vertices
|
||||
for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) {
|
||||
getWrenchVertex(vertex_index).update();
|
||||
|
||||
//TODO: should we enable update on drag vertices?
|
||||
//getDragVertex(vertex_index).update();
|
||||
}
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
|
|
@ -163,6 +176,9 @@ public: //methods
|
|||
}
|
||||
void setKinematics(const Kinematics::State& state)
|
||||
{
|
||||
if (VectorMath::hasNan(state.twist.linear))
|
||||
Utils::DebugBreak();
|
||||
|
||||
kinematics_.setState(state);
|
||||
}
|
||||
const Kinematics::State& getInitialKinematics() const
|
||||
|
|
|
|||
|
|
@ -41,14 +41,14 @@ public:
|
|||
initial_position_ = position;
|
||||
initial_normal_ = normal;
|
||||
drag_factor_ = drag_factor;
|
||||
|
||||
PhysicsBodyVertex::reset();
|
||||
}
|
||||
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
position_ = initial_position_;
|
||||
normal_ = initial_normal_;
|
||||
|
||||
|
|
@ -57,6 +57,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
setWrench(current_wrench_);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
|
|
|||
|
|
@ -10,16 +10,46 @@
|
|||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class PhysicsEngineBase : public UpdatableContainer<PhysicsBody*> {
|
||||
class PhysicsEngineBase : public UpdatableObject {
|
||||
public:
|
||||
//force derived classes to define this
|
||||
virtual void reset() override = 0;
|
||||
virtual void update() override = 0;
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//default nothing to report for physics engine
|
||||
}
|
||||
|
||||
//TODO: reduce copy-past from UpdatableContainer which has same code
|
||||
/********************** Container interface **********************/
|
||||
typedef PhysicsBody* TUpdatableObjectPtr;
|
||||
typedef vector<TUpdatableObjectPtr> MembersContainer;
|
||||
typedef typename MembersContainer::iterator iterator;
|
||||
typedef typename MembersContainer::const_iterator const_iterator;
|
||||
typedef typename MembersContainer::value_type value_type;
|
||||
|
||||
iterator begin() { return members_.begin(); }
|
||||
iterator end() { return members_.end(); }
|
||||
const_iterator begin() const { return members_.begin(); }
|
||||
const_iterator end() const { return members_.end(); }
|
||||
uint size() const { return static_cast<uint>(members_.size()); }
|
||||
const TUpdatableObjectPtr &at(uint index) const { members_.at(index); }
|
||||
TUpdatableObjectPtr &at(uint index) { return members_.at(index); }
|
||||
//allow to override membership modifications
|
||||
virtual void clear() { members_.clear(); }
|
||||
virtual void insert(TUpdatableObjectPtr member) { members_.push_back(member); }
|
||||
virtual void erase_remove(TUpdatableObjectPtr obj) {
|
||||
members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); }
|
||||
|
||||
private:
|
||||
MembersContainer members_;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
#include "PhysicsEngineBase.hpp"
|
||||
#include "PhysicsBody.hpp"
|
||||
#include "common/common_utils/ScheduledExecutor.hpp"
|
||||
//#include "common/DebugClock.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
|
@ -27,11 +28,12 @@ public:
|
|||
{
|
||||
World::clear();
|
||||
|
||||
//clock_ = std::make_shared<msr::airlib::DebugClock>(3E-3f);
|
||||
//msr::airlib::ClockFactory::get(clock_);
|
||||
|
||||
physics_engine_ = physics_engine;
|
||||
if (physics_engine)
|
||||
physics_engine_->clear();
|
||||
|
||||
World::reset();
|
||||
}
|
||||
|
||||
//override updatable interface so we can synchronize physics engine
|
||||
|
|
@ -46,6 +48,9 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
//if (clock_)
|
||||
// clock_->step();
|
||||
|
||||
//first update our objects
|
||||
UpdatableContainer::update();
|
||||
|
||||
|
|
@ -115,17 +120,20 @@ private:
|
|||
update();
|
||||
}
|
||||
catch(const std::exception& ex) {
|
||||
Utils::DebugBreak();
|
||||
Utils::log(Utils::stringf("Exception occurred while updating world: %s", ex.what()), Utils::kLogLevelError);
|
||||
}
|
||||
catch(...) {
|
||||
Utils::DebugBreak();
|
||||
Utils::log("Exception occurred while updating world", Utils::kLogLevelError);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
PhysicsEngineBase* physics_engine_ = nullptr;
|
||||
|
||||
//std::shared_ptr<msr::airlib::DebugClock> clock_;
|
||||
common_utils::ScheduledExecutor executor_;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -29,8 +29,6 @@ public:
|
|||
{
|
||||
ground_truth_.kinematics = kinematics;
|
||||
ground_truth_.environment = environment;
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
const GroundTruth& getGroundTruth() const
|
||||
|
|
|
|||
|
|
@ -72,6 +72,8 @@ public:
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
UpdatableObject::reset();
|
||||
|
||||
for (auto& pair : sensors_) {
|
||||
pair.second->reset();
|
||||
}
|
||||
|
|
@ -79,6 +81,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
for (auto& pair : sensors_) {
|
||||
pair.second->update();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,6 +35,8 @@ public:
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
BarometerBase::reset();
|
||||
|
||||
pressure_factor_.reset();
|
||||
correlated_noise_.reset();
|
||||
uncorrelated_noise_.reset();
|
||||
|
|
@ -47,6 +49,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
BarometerBase::update();
|
||||
|
||||
freq_limiter_.update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
|
|
|
|||
|
|
@ -32,6 +32,8 @@ public: //methods
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
GpsBase::reset();
|
||||
|
||||
freq_limiter_.reset();
|
||||
delay_line_.reset();
|
||||
|
||||
|
|
@ -43,6 +45,8 @@ public: //methods
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
GpsBase::update();
|
||||
|
||||
freq_limiter_.update();
|
||||
eph_filter.update();
|
||||
epv_filter.update();
|
||||
|
|
|
|||
|
|
@ -24,6 +24,8 @@ public:
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
ImuBase::reset();
|
||||
|
||||
last_time_ = clock()->nowNanos();
|
||||
|
||||
state_.gyroscope_bias = params_.gyro.turn_on_bias;
|
||||
|
|
@ -34,6 +36,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
ImuBase::update();
|
||||
|
||||
updateOutput();
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
|
|
|||
|
|
@ -30,6 +30,8 @@ public:
|
|||
//*** Start: UpdatableObject implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
MagnetometerBase::reset();
|
||||
|
||||
//Ground truth is reset before sensors are reset
|
||||
updateReference(getGroundTruth());
|
||||
noise_vec_.reset();
|
||||
|
|
@ -42,6 +44,8 @@ public:
|
|||
|
||||
virtual void update() override
|
||||
{
|
||||
MagnetometerBase::update();
|
||||
|
||||
freq_limiter_.update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
|
|
|
|||
|
|
@ -35,8 +35,6 @@ public:
|
|||
createDragVertices();
|
||||
|
||||
initSensors(*params_, getKinematics(), getEnvironment());
|
||||
|
||||
MultiRotor::reset();
|
||||
}
|
||||
|
||||
DroneControllerBase* getController()
|
||||
|
|
@ -47,16 +45,12 @@ public:
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
//reset inputs
|
||||
if (getController())
|
||||
getController()->reset();
|
||||
|
||||
//reset rotors, kinematics and environment
|
||||
PhysicsBody::reset();
|
||||
|
||||
//reset rotors after environment is reset
|
||||
for (Rotor& rotor : rotors_)
|
||||
rotor.reset();
|
||||
//reset inputs
|
||||
if (getController())
|
||||
getController()->reset();
|
||||
|
||||
//reset sensors last after their ground truth has been reset
|
||||
resetSensors();
|
||||
|
|
@ -66,6 +60,8 @@ public:
|
|||
{
|
||||
//update forces and environment as a result of last dt
|
||||
PhysicsBody::update();
|
||||
|
||||
//Note that controller gets updated after kinematics gets updated in kinematicsUpdated
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
|
|
|
|||
|
|
@ -48,8 +48,6 @@ public: //methods
|
|||
control_signal_filter_.initialize(params_.control_signal_filter_tc, 0, 0);
|
||||
|
||||
PhysicsBodyVertex::initialize(position, normal); //call base initializer
|
||||
|
||||
Rotor::reset();
|
||||
}
|
||||
|
||||
//0 to 1 - will be scalled to 0 to max_speed
|
||||
|
|
@ -67,11 +65,11 @@ public: //methods
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void reset() override
|
||||
{
|
||||
PhysicsBodyVertex::reset();
|
||||
|
||||
//update environmental factors before we call base
|
||||
updateEnvironmentalFactors();
|
||||
|
||||
PhysicsBodyVertex::reset();
|
||||
|
||||
control_signal_filter_.reset();
|
||||
|
||||
setOutput(output_, params_, control_signal_filter_, turning_direction_);
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@ public:
|
|||
VectorMath::toEulerianAngle(q, pitch, roll, yaw);
|
||||
|
||||
Quaternionr qd = VectorMath::toQuaternion(pitch, roll, yaw);
|
||||
if (qd.w() < 0) {
|
||||
if (std::signbit(qd.w()) != std::signbit(q.w())) {
|
||||
qd.coeffs() = - qd.coeffs();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -74,6 +74,7 @@ int main(int argc, const char* argv[])
|
|||
|
||||
MavLinkDroneController mav_drone;
|
||||
mav_drone.initialize(connection_info, nullptr, is_simulation);
|
||||
mav_drone.reset();
|
||||
mav_drone.start();
|
||||
|
||||
DroneControllerCancelable server_wrapper(&mav_drone);
|
||||
|
|
|
|||
|
|
@ -25,11 +25,12 @@ public:
|
|||
|
||||
DebugPhysicsBody body;
|
||||
body.initialize(initial_kinematics, &environment);
|
||||
body.reset();
|
||||
|
||||
//create physics engine
|
||||
FastPhysicsEngine physics;
|
||||
physics.insert(&body);
|
||||
|
||||
physics.reset();
|
||||
|
||||
//run
|
||||
unsigned int i = 0;
|
||||
|
|
|
|||
|
|
@ -16,9 +16,11 @@ namespace msr {
|
|||
auto kinematics = Kinematics::State::zero();
|
||||
msr::airlib::Environment::State initial_environment(kinematics.pose.position, GeoPoint(), 0);
|
||||
msr::airlib::Environment environment(initial_environment);
|
||||
environment.reset();
|
||||
|
||||
ImuSimple imu;
|
||||
imu.initialize(&kinematics, &environment);
|
||||
imu.reset();
|
||||
|
||||
float interations = total_duration / period;
|
||||
|
||||
|
|
@ -45,9 +47,11 @@ namespace msr {
|
|||
auto kinematics = Kinematics::State::zero();
|
||||
msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0);
|
||||
msr::airlib::Environment environment(initial_environment);
|
||||
environment.reset();
|
||||
|
||||
BarometerSimple baro;
|
||||
baro.initialize(&kinematics, &environment);
|
||||
baro.reset();
|
||||
|
||||
float interations = total_duration / period;
|
||||
|
||||
|
|
@ -76,9 +80,11 @@ namespace msr {
|
|||
auto kinematics = Kinematics::State::zero();
|
||||
msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0);
|
||||
msr::airlib::Environment environment(initial_environment);
|
||||
environment.reset();
|
||||
|
||||
BarometerSimple baro;
|
||||
baro.initialize(&kinematics, &environment);
|
||||
baro.reset();
|
||||
|
||||
float interations_20s = 20.0f / period;
|
||||
|
||||
|
|
@ -132,8 +138,11 @@ namespace msr {
|
|||
kinematics.pose.orientation = VectorMath::toQuaternion(0, 0, yaw);
|
||||
msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0);
|
||||
msr::airlib::Environment environment(initial_environment);
|
||||
environment.reset();
|
||||
|
||||
MagnetometerSimple mag;
|
||||
mag.initialize(&kinematics, &environment);
|
||||
mag.reset();
|
||||
|
||||
for (auto i = 0; i < interations; ++i) {
|
||||
const auto& output = mag.getOutput();
|
||||
|
|
@ -178,8 +187,11 @@ namespace msr {
|
|||
kinematics.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw);
|
||||
msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0);
|
||||
msr::airlib::Environment environment(initial_environment);
|
||||
environment.reset();
|
||||
|
||||
MagnetometerSimple mag;
|
||||
mag.initialize(&kinematics, &environment);
|
||||
mag.reset();
|
||||
|
||||
for (auto i = 0; i < interations; ++i) {
|
||||
const auto& output = mag.getOutput();
|
||||
|
|
@ -211,8 +223,11 @@ namespace msr {
|
|||
kinematics.pose.orientation = VectorMath::toQuaternion(0, 0, 0);
|
||||
msr::airlib::Environment::State initial_environment(kinematics.pose.position, GeoPoint(), 0);
|
||||
msr::airlib::Environment environment(initial_environment);
|
||||
environment.reset();
|
||||
|
||||
MagnetometerSimple mag;
|
||||
mag.initialize(&kinematics, &environment);
|
||||
mag.reset();
|
||||
|
||||
for (float lat = -90; lat < 90; lat++)
|
||||
{
|
||||
|
|
|
|||
Разница между файлами не показана из-за своего большого размера
Загрузить разницу
|
|
@ -273,6 +273,11 @@ bool MultiRotorConnector::isApiServerStarted()
|
|||
//*** Start: UpdatableState implementation ***//
|
||||
void MultiRotorConnector::reset()
|
||||
{
|
||||
VehicleConnectorBase::reset();
|
||||
|
||||
//TODO: should this be done in MultiRotor.hpp
|
||||
//controller_->reset();
|
||||
|
||||
rc_data_ = RCData();
|
||||
vehicle_pawn_->reset(); //we do flier resetPose so that flier is placed back without collisons
|
||||
vehicle_.reset();
|
||||
|
|
@ -280,6 +285,8 @@ void MultiRotorConnector::reset()
|
|||
|
||||
void MultiRotorConnector::update()
|
||||
{
|
||||
VehicleConnectorBase::update();
|
||||
|
||||
//this is high frequency physics tick, flier gets ticked at rendering frame rate
|
||||
vehicle_.update();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,5 @@
|
|||
#include "SimModeWorldBase.h"
|
||||
|
||||
|
||||
const char ASimModeWorldBase::kUsageScenarioComputerVision[] = "ComputerVision";
|
||||
|
||||
void ASimModeWorldBase::BeginPlay()
|
||||
|
|
@ -51,7 +50,6 @@ void ASimModeWorldBase::createWorld()
|
|||
new msr::airlib::FastPhysicsEngine()
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
else {
|
||||
physics_engine_.release();
|
||||
|
|
@ -59,13 +57,15 @@ void ASimModeWorldBase::createWorld()
|
|||
}
|
||||
|
||||
world_.initialize(physics_engine_.get());
|
||||
reporter_.initialize(false);
|
||||
|
||||
//add default objects to world
|
||||
reporter_.initialize(false);
|
||||
world_.insert(&reporter_);
|
||||
|
||||
for(size_t vi = 0; vi < vehicles_.size(); vi++)
|
||||
world_.insert(vehicles_.at(vi).get());
|
||||
|
||||
world_.reset();
|
||||
|
||||
for (auto& vehicle : vehicles_)
|
||||
vehicle->beginPlay();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -122,12 +122,12 @@ void AVehiclePawnBase::initialize()
|
|||
initial_state_.collisons_enabled = EnableCollisons;
|
||||
initial_state_.passthrough_enabled = EnablePassthroughOnCollisons;
|
||||
|
||||
|
||||
initial_state_.collison_info = CollisionInfo();
|
||||
|
||||
initial_state_.was_last_move_teleport = false;
|
||||
initial_state_.was_last_move_teleport = canTeleportWhileMove();
|
||||
|
||||
//call derived class reset which in turn should call base class reset
|
||||
reset();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,6 @@
|
|||
rd /s/q AirLib\lib
|
||||
rd /s/q AirLib\deps\MavLinkCom
|
||||
rd /s/q AirLib\deps\rpclib
|
||||
rd /s/q external\rpclib\build
|
||||
msbuild /p:Platform=x64 /p:Configuration=Debug AirSim.sln /t:Clean
|
||||
if ERRORLEVEL 1 goto :buildfailed
|
||||
|
|
|
|||
Загрузка…
Ссылка в новой задаче