MechanismPositionConfig.hpp

Go to the documentation of this file.

// Copyright (c) 2026 Yet Another Software Suite
// SPDX-License-Identifier: LGPL-3.0-or-later
 
#pragma once
 
#include <cmath>
#include <functional>
#include <optional>
#include <utility>
 
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/math/geometry/Transform3d.hpp>
#include <wpi/math/geometry/Translation3d.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/length.hpp>
 
namespace yams::mechanisms::config {
 
class MechanismPositionConfig {
 public:
  enum class Plane {
    XZ,
    YZ,
    XY
  };
 
  MechanismPositionConfig() = default;
 
  MechanismPositionConfig& WithRelativePosition(const wpi::math::Translation3d& robotToMechanism) {
    m_robotToMechanism = robotToMechanism;
    return *this;
  }
 
  MechanismPositionConfig& WithRelativePosition(const wpi::math::Transform3d& robotToMechanism) {
    m_robotToMechanism = robotToMechanism.Translation();
    m_mountRotation = robotToMechanism.Rotation();
    return *this;
  }
 
  MechanismPositionConfig& WithMovementPlane(Plane plane) {
    m_plane = plane;
    return *this;
  }
 
  MechanismPositionConfig& WithParent(std::function<wpi::math::Pose3d()> parentPose) {
    m_parent = std::move(parentPose);
    return *this;
  }
 
  std::optional<wpi::math::Translation3d> GetRelativePosition() const { return m_robotToMechanism; }
 
  Plane GetMovementPlane() const { return m_plane; }
 
  wpi::math::Pose3d GetMountPose() const {
    wpi::math::Pose3d base = m_parent ? m_parent() : wpi::math::Pose3d{};
    return base.TransformBy(wpi::math::Transform3d{m_robotToMechanism.value_or(wpi::math::Translation3d{}),
                                                   m_mountRotation});
  }
 
  wpi::math::Pose3d GetRotatedPose(wpi::units::radian_t angle) const {
    return GetMountPose().TransformBy(
        wpi::math::Transform3d{wpi::math::Translation3d{}, RotationInPlane(angle)});
  }
 
  wpi::math::Pose3d GetTranslatedPose(wpi::units::meter_t distance, wpi::units::radian_t angle) const {
    const wpi::units::meter_t along = distance * std::cos(angle.value());
    const wpi::units::meter_t across = distance * std::sin(angle.value());
    wpi::math::Translation3d slide;
    switch (m_plane) {
      case Plane::XZ:
        slide = wpi::math::Translation3d{along, wpi::units::meter_t{0}, across};
        break;
      case Plane::YZ:
        slide = wpi::math::Translation3d{wpi::units::meter_t{0}, along, across};
        break;
      case Plane::XY:
        slide = wpi::math::Translation3d{along, across, wpi::units::meter_t{0}};
        break;
    }
    return GetMountPose().TransformBy(wpi::math::Transform3d{slide, wpi::math::Rotation3d{}});
  }
 
  wpi::math::Rotation3d RotationInPlane(wpi::units::radian_t angle) const {
    switch (m_plane) {
      case Plane::YZ:
        // +Y turns toward +Z: a positive roll about +X.
        return wpi::math::Rotation3d{angle, wpi::units::radian_t{0}, wpi::units::radian_t{0}};
      case Plane::XY:
        // +X turns toward +Y: a positive yaw about +Z.
        return wpi::math::Rotation3d{wpi::units::radian_t{0}, wpi::units::radian_t{0}, angle};
      case Plane::XZ:
      default:
        // +X turns toward +Z: a negative pitch about +Y.
        return wpi::math::Rotation3d{wpi::units::radian_t{0}, -angle, wpi::units::radian_t{0}};
    }
  }
 
 private:
  std::optional<wpi::math::Translation3d> m_robotToMechanism;
  wpi::math::Rotation3d m_mountRotation;
  Plane m_plane{Plane::XZ};
  std::function<wpi::math::Pose3d()> m_parent;
};
 
}  // namespace yams::mechanisms::config