line_point_constraint.hpp

// Copyright (c) TrajoptLib contributors
 
#pragma once
 
#include <cassert>
#include <utility>
 
#include <sleipnir/autodiff/variable.hpp>
#include <sleipnir/optimization/problem.hpp>
 
#include "trajopt/constraint/detail/line_point_squared_distance.hpp"
#include "trajopt/geometry/pose2.hpp"
#include "trajopt/geometry/translation2.hpp"
#include "trajopt/util/symbol_exports.hpp"
 
namespace trajopt {

class TRAJOPT_DLLEXPORT LinePointConstraint {
 public:
  explicit LinePointConstraint(Translation2d robot_line_start,
                               Translation2d robot_line_end,
                               Translation2d field_point, double min_distance)
      : m_robot_line_start{std::move(robot_line_start)},
        m_robot_line_end{std::move(robot_line_end)},
        m_field_point{std::move(field_point)},
        m_min_distance{min_distance} {
    assert(min_distance >= 0.0);
  }

  void apply(
      slp::Problem<double>& problem, const Pose2v<double>& pose,
      [[maybe_unused]] const Translation2v<double>& linear_velocity,
      [[maybe_unused]] const slp::Variable<double>& angular_velocity,
      [[maybe_unused]] const Translation2v<double>& linear_acceleration,
      [[maybe_unused]] const slp::Variable<double>& angular_acceleration) {
    auto line_start =
        pose.translation() + m_robot_line_start.rotate_by(pose.rotation());
    auto line_end =
        pose.translation() + m_robot_line_end.rotate_by(pose.rotation());
    auto squared_distance = detail::line_point_squared_distance(
        line_start, line_end, m_field_point);
    problem.subject_to(squared_distance >= m_min_distance * m_min_distance);
  }
 
 private:
  Translation2d m_robot_line_start;
  Translation2d m_robot_line_end;
  Translation2d m_field_point;
  double m_min_distance;
};
 
}  // namespace trajopt