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 <xtensor/xmath.hpp>
#include <xtensor/xmasked_view.hpp>
#include <xtensor/xview.hpp>
#include <xtensor/xnoalias.hpp>
 
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
 
namespace mppi
{
 
class MotionModel
{
public:
  MotionModel() = default;
 
  virtual ~MotionModel() = default;
 
  virtual void predict(models::State & state)
  {
    using namespace xt::placeholders;  // NOLINT
    xt::noalias(xt::view(state.vx, xt::all(), xt::range(1, _))) =
      xt::view(state.cvx, xt::all(), xt::range(0, -1));
 
    xt::noalias(xt::view(state.wz, xt::all(), xt::range(1, _))) =
      xt::view(state.cwz, xt::all(), xt::range(0, -1));
 
    if (isHolonomic()) {
      xt::noalias(xt::view(state.vy, xt::all(), xt::range(1, _))) =
        xt::view(state.cvy, xt::all(), xt::range(0, -1));
    }
  }
 
  virtual bool isHolonomic() = 0;
 
  virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {}
};
 
class AckermannMotionModel : public MotionModel
{
public:
  explicit AckermannMotionModel(ParametersHandler * param_handler, const std::string & name)
  {
    auto getParam = param_handler->getParamGetter(name + ".AckermannConstraints");
    getParam(min_turning_r_, "min_turning_r", 0.2);
  }
 
  bool isHolonomic() override
  {
    return false;
  }
 
  void applyConstraints(models::ControlSequence & control_sequence) override
  {
    auto & wz = control_sequence.wz;
    auto abs_vx = xt::fabs(control_sequence.vx);
    auto abs_wz = xt::fabs(wz);
 
    for (size_t i = 0; i < wz.size(); ++i) {
      if ((abs_vx[i] / abs_wz[i]) < min_turning_r_) {
        wz[i] = std::copysign(abs_vx[i] / min_turning_r_, wz[i]);
      }
    }
  }
 
  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_