Program Listing for File omnidirectional_drive_model.hpp

↰ Return to documentation for file (include/beluga/motion/omnidirectional_drive_model.hpp)

// Copyright 2023 Ekumen, Inc.
//
// 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 BELUGA_MOTION_OMNIDIRECTIONAL_DRIVE_MODEL_HPP
#define BELUGA_MOTION_OMNIDIRECTIONAL_DRIVE_MODEL_HPP

#include <optional>
#include <random>
#include <tuple>
#include <type_traits>

#include <beluga/type_traits/tuple_traits.hpp>

#include <sophus/se2.hpp>
#include <sophus/so2.hpp>

namespace beluga {

struct OmnidirectionalDriveModelParam {

  double rotation_noise_from_rotation;

  double rotation_noise_from_translation;

  double translation_noise_from_translation;

  double translation_noise_from_rotation;

  double strafe_noise_from_translation;

  double distance_threshold = 0.01;
};


class OmnidirectionalDriveModel {
 public:
  using control_type = std::tuple<Sophus::SE2d, Sophus::SE2d>;
  using state_type = Sophus::SE2d;

  using param_type = OmnidirectionalDriveModelParam;


  explicit OmnidirectionalDriveModel(const param_type& params) : params_{params} {}


  template <class Control, typename = common_tuple_type_t<Control, control_type>>
  [[nodiscard]] auto operator()(Control&& action) const {
    const auto& [pose, previous_pose] = action;

    const auto translation = pose.translation() - previous_pose.translation();
    const double distance = translation.norm();
    const double distance_variance = distance * distance;

    const auto& previous_orientation = previous_pose.so2();
    const auto& current_orientation = pose.so2();
    const auto rotation = current_orientation * previous_orientation.inverse();

    const auto heading_rotation = Sophus::SO2d{std::atan2(translation.y(), translation.x())};
    const auto first_rotation =
        distance > params_.distance_threshold ? heading_rotation * previous_orientation.inverse() : Sophus::SO2d{};

    using DistributionParam = typename std::normal_distribution<double>::param_type;
    const auto rotation_params = DistributionParam{
        rotation.log(), std::sqrt(
                            params_.rotation_noise_from_rotation * rotation_variance(rotation) +
                            params_.rotation_noise_from_translation * distance_variance)};

    const auto translation_params = DistributionParam{
        distance, std::sqrt(
                      params_.translation_noise_from_translation * distance_variance +
                      params_.translation_noise_from_rotation * rotation_variance(rotation))};

    const auto strafe_params = DistributionParam{
        0.0, std::sqrt(
                 params_.strafe_noise_from_translation * distance_variance +
                 params_.translation_noise_from_rotation * rotation_variance(rotation))};

    return [=](const state_type& state, auto& gen) {
      static thread_local auto distribution = std::normal_distribution<double>{};
      // This is an implementation based on the same set of parameters that is used in
      // nav2's omni_motion_model. To simplify the implementation, the following
      // variable substitutions were performed:
      // - first_rotation = delta_bearing - previous_orientation
      // - second_rotation = delta_rot_hat - first_rotation
      const auto second_rotation = Sophus::SO2d{distribution(gen, rotation_params)} * first_rotation.inverse();
      const auto translation =
          Eigen::Vector2d{distribution(gen, translation_params), -distribution(gen, strafe_params)};
      return state * Sophus::SE2d{first_rotation, Eigen::Vector2d{0.0, 0.0}} *
             Sophus::SE2d{second_rotation, translation};
    };
  }

 private:
  param_type params_;

  static double rotation_variance(const Sophus::SO2d& rotation) {
    // Treat backward and forward motion symmetrically for the noise models.
    const auto flipped_rotation = rotation * Sophus::SO2d{Sophus::Constants<double>::pi()};
    const auto delta = std::min(std::abs(rotation.log()), std::abs(flipped_rotation.log()));
    return delta * delta;
  }
};

}  // namespace beluga

#endif