point_at_constraint.hpp

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

class TRAJOPT_DLLEXPORT PointAtConstraint {
 public:
  explicit PointAtConstraint(Translation2d field_point,
                             double heading_tolerance, bool flip = false)
      : m_field_point{std::move(field_point)},
        m_heading_tolerance{heading_tolerance},
        m_flip{flip} {
    assert(m_heading_tolerance >= 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) {
    // dx,dy = desired heading
    // ux,uy = unit vector of desired heading
    // hx,hy = heading
    // dot = dot product of ux,uy and hx,hy
    //
    // constrain dot to cos(1.0), which is colinear
    // and cos(theta_tolerance)
    auto dx = m_field_point.x() - pose.x();
    auto dy = m_field_point.y() - pose.y();
    auto dot = pose.rotation().cos() * dx + pose.rotation().sin() * dy;
    if (!m_flip) {
      // dot close to 1 * hypot (point toward)
      problem.subject_to(dot >=
                         std::cos(m_heading_tolerance) * slp::hypot(dx, dy));
    } else {
      // dot close to -1 * hypot (point away)
      problem.subject_to(dot <=
                         -std::cos(m_heading_tolerance) * slp::hypot(dx, dy));
    }
  }
 
 private:
  Translation2d m_field_point;
  double m_heading_tolerance;
  bool m_flip;
};
 
}  // namespace trajopt