Program Listing for File motion_models.hpp
↰ Return to documentation for file (include/nav2_mppi_controller/motion_models.hpp
)
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_MPPI_CONTROLLER__MOTION_MODELS_HPP_
#define NAV2_MPPI_CONTROLLER__MOTION_MODELS_HPP_
#include <cstdint>
#include <string>
#include "nav2_mppi_controller/models/control_sequence.hpp"
#include "nav2_mppi_controller/models/state.hpp"
#include "nav2_mppi_controller/models/constraints.hpp"
// xtensor creates warnings that needs to be ignored as we are building with -Werror
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
#pragma GCC diagnostic ignored "-Wstringop-overflow"
#include <xtensor/xmath.hpp>
#include <xtensor/xmasked_view.hpp>
#include <xtensor/xview.hpp>
#include <xtensor/xnoalias.hpp>
#pragma GCC diagnostic pop
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
namespace mppi
{
class MotionModel
{
public:
MotionModel() = default;
virtual ~MotionModel() = default;
void initialize(const models::ControlConstraints & control_constraints, float model_dt)
{
control_constraints_ = control_constraints;
model_dt_ = model_dt;
}
virtual void predict(models::State & state)
{
// Previously completed via tensor views, but found to be 10x slower
// using namespace xt::placeholders; // NOLINT
// xt::noalias(xt::view(state.vx, xt::all(), xt::range(1, _))) =
// xt::noalias(xt::view(state.cvx, xt::all(), xt::range(0, -1)));
// xt::noalias(xt::view(state.wz, xt::all(), xt::range(1, _))) =
// xt::noalias(xt::view(state.cwz, xt::all(), xt::range(0, -1)));
// if (isHolonomic()) {
// xt::noalias(xt::view(state.vy, xt::all(), xt::range(1, _))) =
// xt::noalias(xt::view(state.cvy, xt::all(), xt::range(0, -1)));
// }
const bool is_holo = isHolonomic();
float max_delta_vx = model_dt_ * control_constraints_.ax_max;
float min_delta_vx = model_dt_ * control_constraints_.ax_min;
float max_delta_vy = model_dt_ * control_constraints_.ay_max;
float max_delta_wz = model_dt_ * control_constraints_.az_max;
for (unsigned int i = 0; i != state.vx.shape(0); i++) {
float vx_last = state.vx(i, 0);
float vy_last = state.vy(i, 0);
float wz_last = state.wz(i, 0);
for (unsigned int j = 1; j != state.vx.shape(1); j++) {
float & cvx_curr = state.cvx(i, j - 1);
cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx);
state.vx(i, j) = cvx_curr;
vx_last = cvx_curr;
float & cwz_curr = state.cwz(i, j - 1);
cwz_curr = std::clamp(cwz_curr, wz_last - max_delta_wz, wz_last + max_delta_wz);
state.wz(i, j) = cwz_curr;
wz_last = cwz_curr;
if (is_holo) {
float & cvy_curr = state.cvy(i, j - 1);
cvy_curr = std::clamp(cvy_curr, vy_last - max_delta_vy, vy_last + max_delta_vy);
state.vy(i, j) = cvy_curr;
vy_last = cvy_curr;
}
}
}
}
virtual bool isHolonomic() = 0;
virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {}
protected:
float model_dt_{0.0};
models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
0.0f};
};
class AckermannMotionModel : public MotionModel
{
public:
explicit AckermannMotionModel(ParametersHandler * param_handler)
{
auto getParam = param_handler->getParamGetter("AckermannConstraints");
getParam(min_turning_r_, "min_turning_r", 0.2);
}
bool isHolonomic() override
{
return false;
}
void applyConstraints(models::ControlSequence & control_sequence) override
{
auto & vx = control_sequence.vx;
auto & wz = control_sequence.wz;
auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) < min_turning_r_);
view = xt::sign(wz) * vx / min_turning_r_;
}
float getMinTurningRadius() {return min_turning_r_;}
private:
float min_turning_r_{0};
};
class DiffDriveMotionModel : public MotionModel
{
public:
DiffDriveMotionModel() = default;
bool isHolonomic() override
{
return false;
}
};
class OmniMotionModel : public MotionModel
{
public:
OmniMotionModel() = default;
bool isHolonomic() override
{
return true;
}
};
} // namespace mppi
#endif // NAV2_MPPI_CONTROLLER__MOTION_MODELS_HPP_