mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Update to wpilib 2023 beta 7 (#607)
We now need platform specific jars -- reworks actions to support that. Currently only generates 32 bit pi images.
This commit is contained in:
shueja-personal
@@ -1,159 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
namespace {
|
||||
|
||||
/**
|
||||
* Applies the hat operator to a rotation vector.
|
||||
*
|
||||
* It takes a rotation vector and returns the corresponding matrix
|
||||
* representation of the Lie algebra element (a 3x3 rotation matrix).
|
||||
*
|
||||
* @param rotation The rotation vector.
|
||||
* @return The rotation vector as a 3x3 rotation matrix.
|
||||
*/
|
||||
Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
|
||||
// Given a rotation vector <a, b, c>,
|
||||
// [ 0 -c b]
|
||||
// Omega = [ c 0 -a]
|
||||
// [-b a 0]
|
||||
return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
|
||||
{rotation(2), 0.0, -rotation(0)},
|
||||
{-rotation(1), rotation(0), 0.0}};
|
||||
}
|
||||
} // namespace
|
||||
|
||||
Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
|
||||
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
|
||||
|
||||
Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
|
||||
Rotation3d rotation)
|
||||
: m_translation(x, y, z), m_rotation(std::move(rotation)) {}
|
||||
|
||||
Pose3d Pose3d::operator+(const Transform3d& other) const {
|
||||
return TransformBy(other);
|
||||
}
|
||||
|
||||
Transform3d Pose3d::operator-(const Pose3d& other) const {
|
||||
const auto pose = this->RelativeTo(other);
|
||||
return Transform3d{pose.Translation(), pose.Rotation()};
|
||||
}
|
||||
|
||||
bool Pose3d::operator==(const Pose3d& other) const {
|
||||
return m_translation == other.m_translation && m_rotation == other.m_rotation;
|
||||
}
|
||||
|
||||
bool Pose3d::operator!=(const Pose3d& other) const {
|
||||
return !operator==(other);
|
||||
}
|
||||
|
||||
Pose3d Pose3d::TransformBy(const Transform3d& other) const {
|
||||
return {m_translation + (other.Translation().RotateBy(m_rotation)),
|
||||
m_rotation + other.Rotation()};
|
||||
}
|
||||
|
||||
Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
|
||||
const Transform3d transform{other, *this};
|
||||
return {transform.Translation(), transform.Rotation()};
|
||||
}
|
||||
|
||||
Pose3d Pose3d::Exp(const Twist3d& twist) const {
|
||||
Matrixd<3, 3> Omega = RotationVectorToMatrix(
|
||||
Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
|
||||
Matrixd<3, 3> OmegaSq = Omega * Omega;
|
||||
|
||||
double thetaSq =
|
||||
(twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
|
||||
|
||||
// Get left Jacobian of SO3. See first line in right column of
|
||||
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
|
||||
Matrixd<3, 3> J;
|
||||
if (thetaSq < 1E-9 * 1E-9) {
|
||||
// V = I + 0.5ω
|
||||
J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
|
||||
} else {
|
||||
double theta = std::sqrt(thetaSq);
|
||||
// J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
|
||||
J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
|
||||
(theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
|
||||
}
|
||||
|
||||
// Get translation component
|
||||
Vectord<3> translation =
|
||||
J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
|
||||
|
||||
const Transform3d transform{Translation3d{units::meter_t{translation(0)},
|
||||
units::meter_t{translation(1)},
|
||||
units::meter_t{translation(2)}},
|
||||
Rotation3d{twist.rx, twist.ry, twist.rz}};
|
||||
|
||||
return *this + transform;
|
||||
}
|
||||
|
||||
Twist3d Pose3d::Log(const Pose3d& end) const {
|
||||
const auto transform = end.RelativeTo(*this);
|
||||
|
||||
Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
|
||||
|
||||
Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
|
||||
Matrixd<3, 3> OmegaSq = Omega * Omega;
|
||||
|
||||
double thetaSq = rotVec.squaredNorm();
|
||||
|
||||
// Get left Jacobian inverse of SO3. See fourth line in right column of
|
||||
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
|
||||
Matrixd<3, 3> Jinv;
|
||||
if (thetaSq < 1E-9 * 1E-9) {
|
||||
// J⁻¹ = I − 0.5ω + 1/12 ω²
|
||||
Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
|
||||
} else {
|
||||
double theta = std::sqrt(thetaSq);
|
||||
double halfTheta = 0.5 * theta;
|
||||
|
||||
// J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
|
||||
Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
|
||||
(1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
|
||||
thetaSq * OmegaSq;
|
||||
}
|
||||
|
||||
// Get dtranslation component
|
||||
Vectord<3> dtranslation =
|
||||
Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
|
||||
transform.Z().value()};
|
||||
|
||||
return Twist3d{
|
||||
units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
|
||||
units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
|
||||
units::radian_t{rotVec(1)}, units::radian_t{rotVec(2)}};
|
||||
}
|
||||
|
||||
Pose2d Pose3d::ToPose2d() const {
|
||||
return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
|
||||
}
|
||||
@@ -1,95 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <frc/geometry/Quaternion.h>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Quaternion::Quaternion(double w, double x, double y, double z)
|
||||
: m_r{w}, m_v{x, y, z} {}
|
||||
|
||||
Quaternion Quaternion::operator*(const Quaternion& other) const {
|
||||
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
|
||||
const auto& r1 = m_r;
|
||||
const auto& v1 = m_v;
|
||||
const auto& r2 = other.m_r;
|
||||
const auto& v2 = other.m_v;
|
||||
|
||||
// v₁ x v₂
|
||||
Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
|
||||
v2(0) * v1(2) - v1(0) * v2(2),
|
||||
v1(0) * v2(1) - v2(0) * v1(1)};
|
||||
|
||||
// v = r₁v₂ + r₂v₁ + v₁ x v₂
|
||||
Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
|
||||
|
||||
return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
|
||||
}
|
||||
|
||||
bool Quaternion::operator==(const Quaternion& other) const {
|
||||
return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
|
||||
}
|
||||
|
||||
bool Quaternion::operator!=(const Quaternion& other) const {
|
||||
return !operator==(other);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Inverse() const {
|
||||
return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Normalize() const {
|
||||
double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
|
||||
if (norm == 0.0) {
|
||||
return Quaternion{};
|
||||
} else {
|
||||
return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
|
||||
}
|
||||
}
|
||||
|
||||
double Quaternion::W() const { return m_r; }
|
||||
|
||||
double Quaternion::X() const { return m_v(0); }
|
||||
|
||||
double Quaternion::Y() const { return m_v(1); }
|
||||
|
||||
double Quaternion::Z() const { return m_v(2); }
|
||||
|
||||
Eigen::Vector3d Quaternion::ToRotationVector() const {
|
||||
// See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
|
||||
// Sound State Representation through Encapsulation of Manifolds"
|
||||
//
|
||||
// https://arxiv.org/pdf/1107.1119.pdf
|
||||
double norm = m_v.norm();
|
||||
|
||||
if (norm < 1e-9) {
|
||||
return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v;
|
||||
} else {
|
||||
if (W() < 0.0) {
|
||||
return 2.0 * std::atan2(-norm, -W()) / norm * m_v;
|
||||
} else {
|
||||
return 2.0 * std::atan2(norm, W()) / norm * m_v;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,248 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/LU>
|
||||
#include <Eigen/QR>
|
||||
#include <frc/fmt/Eigen.h>
|
||||
#include <frc/geometry/Rotation3d.h>
|
||||
#include <units/math.h>
|
||||
#include <wpi/numbers>
|
||||
|
||||
#include "wpimath/MathShared.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Rotation3d::Rotation3d(const Quaternion& q) { m_q = q.Normalize(); }
|
||||
|
||||
Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
|
||||
units::radian_t yaw) {
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
|
||||
double cr = units::math::cos(roll * 0.5);
|
||||
double sr = units::math::sin(roll * 0.5);
|
||||
|
||||
double cp = units::math::cos(pitch * 0.5);
|
||||
double sp = units::math::sin(pitch * 0.5);
|
||||
|
||||
double cy = units::math::cos(yaw * 0.5);
|
||||
double sy = units::math::sin(yaw * 0.5);
|
||||
|
||||
m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
|
||||
cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
|
||||
}
|
||||
|
||||
Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
|
||||
double norm = axis.norm();
|
||||
if (norm == 0.0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
|
||||
Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
|
||||
m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
|
||||
}
|
||||
|
||||
Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
|
||||
const auto& R = rotationMatrix;
|
||||
|
||||
// Require that the rotation matrix is special orthogonal. This is true if the
|
||||
// matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
|
||||
if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
|
||||
std::string msg =
|
||||
fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
|
||||
|
||||
wpi::math::MathSharedStore::ReportError(msg);
|
||||
throw std::domain_error(msg);
|
||||
}
|
||||
if (R.determinant() != 1.0) {
|
||||
std::string msg = fmt::format(
|
||||
"Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n",
|
||||
R);
|
||||
|
||||
wpi::math::MathSharedStore::ReportError(msg);
|
||||
throw std::domain_error(msg);
|
||||
}
|
||||
|
||||
// Turn rotation matrix into a quaternion
|
||||
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
|
||||
double trace = R(0, 0) + R(1, 1) + R(2, 2);
|
||||
double w;
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
|
||||
if (trace > 0.0) {
|
||||
double s = 0.5 / std::sqrt(trace + 1.0);
|
||||
w = 0.25 / s;
|
||||
x = (R(2, 1) - R(1, 2)) * s;
|
||||
y = (R(0, 2) - R(2, 0)) * s;
|
||||
z = (R(1, 0) - R(0, 1)) * s;
|
||||
} else {
|
||||
if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
|
||||
double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
|
||||
w = (R(2, 1) - R(1, 2)) / s;
|
||||
x = 0.25 * s;
|
||||
y = (R(0, 1) + R(1, 0)) / s;
|
||||
z = (R(0, 2) + R(2, 0)) / s;
|
||||
} else if (R(1, 1) > R(2, 2)) {
|
||||
double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
|
||||
w = (R(0, 2) - R(2, 0)) / s;
|
||||
x = (R(0, 1) + R(1, 0)) / s;
|
||||
y = 0.25 * s;
|
||||
z = (R(1, 2) + R(2, 1)) / s;
|
||||
} else {
|
||||
double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
|
||||
w = (R(1, 0) - R(0, 1)) / s;
|
||||
x = (R(0, 2) + R(2, 0)) / s;
|
||||
y = (R(1, 2) + R(2, 1)) / s;
|
||||
z = 0.25 * s;
|
||||
}
|
||||
}
|
||||
|
||||
m_q = Quaternion{w, x, y, z};
|
||||
}
|
||||
|
||||
Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
|
||||
double dot = initial.dot(final);
|
||||
double normProduct = initial.norm() * final.norm();
|
||||
double dotNorm = dot / normProduct;
|
||||
|
||||
if (dotNorm > 1.0 - 1E-9) {
|
||||
// If the dot product is 1, the two vectors point in the same direction so
|
||||
// there's no rotation. The default initialization of m_q will work.
|
||||
return;
|
||||
} else if (dotNorm < -1.0 + 1E-9) {
|
||||
// If the dot product is -1, the two vectors point in opposite directions so
|
||||
// a 180 degree rotation is required. Any orthogonal vector can be used for
|
||||
// it. Q in the QR decomposition is an orthonormal basis, so it contains
|
||||
// orthogonal unit vectors.
|
||||
Eigen::Matrix<double, 3, 1> X = initial;
|
||||
Eigen::HouseholderQR<decltype(X)> qr{X};
|
||||
Eigen::Matrix<double, 3, 3> Q = qr.householderQ();
|
||||
|
||||
// w = std::cos(θ/2) = std::cos(90°) = 0
|
||||
//
|
||||
// For x, y, and z, we use the second column of Q because the first is
|
||||
// parallel instead of orthogonal. The third column would also work.
|
||||
m_q = Quaternion{0.0, Q(0, 1), Q(1, 1), Q(2, 1)};
|
||||
} else {
|
||||
// initial x final
|
||||
Eigen::Vector3d axis{initial(1) * final(2) - final(1) * initial(2),
|
||||
final(0) * initial(2) - initial(0) * final(2),
|
||||
initial(0) * final(1) - final(0) * initial(1)};
|
||||
|
||||
// https://stackoverflow.com/a/11741520
|
||||
m_q = Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
|
||||
}
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
|
||||
return RotateBy(other);
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::operator-(const Rotation3d& other) const {
|
||||
return *this + -other;
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::operator-() const { return Rotation3d{m_q.Inverse()}; }
|
||||
|
||||
Rotation3d Rotation3d::operator*(double scalar) const {
|
||||
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
|
||||
if (m_q.W() >= 0.0) {
|
||||
return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()},
|
||||
2.0 * units::radian_t{scalar * std::acos(m_q.W())}};
|
||||
} else {
|
||||
return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()},
|
||||
2.0 * units::radian_t{scalar * std::acos(-m_q.W())}};
|
||||
}
|
||||
}
|
||||
|
||||
bool Rotation3d::operator==(const Rotation3d& other) const {
|
||||
return m_q == other.m_q;
|
||||
}
|
||||
|
||||
bool Rotation3d::operator!=(const Rotation3d& other) const {
|
||||
return !operator==(other);
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
|
||||
return Rotation3d{other.m_q * m_q};
|
||||
}
|
||||
|
||||
const Quaternion& Rotation3d::GetQuaternion() const { return m_q; }
|
||||
|
||||
units::radian_t Rotation3d::X() const {
|
||||
double w = m_q.W();
|
||||
double x = m_q.X();
|
||||
double y = m_q.Y();
|
||||
double z = m_q.Z();
|
||||
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
|
||||
return units::radian_t{
|
||||
std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
|
||||
}
|
||||
|
||||
units::radian_t Rotation3d::Y() const {
|
||||
double w = m_q.W();
|
||||
double x = m_q.X();
|
||||
double y = m_q.Y();
|
||||
double z = m_q.Z();
|
||||
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
|
||||
double ratio = 2.0 * (w * y - z * x);
|
||||
if (std::abs(ratio) >= 1.0) {
|
||||
return units::radian_t{std::copysign(wpi::numbers::pi / 2.0, ratio)};
|
||||
} else {
|
||||
return units::radian_t{std::asin(ratio)};
|
||||
}
|
||||
}
|
||||
|
||||
units::radian_t Rotation3d::Z() const {
|
||||
double w = m_q.W();
|
||||
double x = m_q.X();
|
||||
double y = m_q.Y();
|
||||
double z = m_q.Z();
|
||||
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
|
||||
return units::radian_t{
|
||||
std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
|
||||
}
|
||||
|
||||
Vectord<3> Rotation3d::Axis() const {
|
||||
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
|
||||
if (norm == 0.0) {
|
||||
return {0.0, 0.0, 0.0};
|
||||
} else {
|
||||
return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm};
|
||||
}
|
||||
}
|
||||
|
||||
units::radian_t Rotation3d::Angle() const {
|
||||
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
|
||||
return units::radian_t{2.0 * std::atan2(norm, m_q.W())};
|
||||
}
|
||||
|
||||
Rotation2d Rotation3d::ToRotation2d() const { return Rotation2d{Z()}; }
|
||||
@@ -1,60 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
#include <frc/geometry/Transform3d.h>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Transform3d::Transform3d(Pose3d initial, Pose3d final) {
|
||||
// We are rotating the difference between the translations
|
||||
// using a clockwise rotation matrix. This transforms the global
|
||||
// delta into a local delta (relative to the initial pose).
|
||||
m_translation = (final.Translation() - initial.Translation())
|
||||
.RotateBy(-initial.Rotation());
|
||||
|
||||
m_rotation = final.Rotation() - initial.Rotation();
|
||||
}
|
||||
|
||||
Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
|
||||
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
|
||||
|
||||
Transform3d Transform3d::Inverse() const {
|
||||
// We are rotating the difference between the translations
|
||||
// using a clockwise rotation matrix. This transforms the global
|
||||
// delta into a local delta (relative to the initial pose).
|
||||
return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
|
||||
}
|
||||
|
||||
Transform3d Transform3d::operator+(const Transform3d& other) const {
|
||||
return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
|
||||
}
|
||||
|
||||
bool Transform3d::operator==(const Transform3d& other) const {
|
||||
return m_translation == other.m_translation && m_rotation == other.m_rotation;
|
||||
}
|
||||
|
||||
bool Transform3d::operator!=(const Transform3d& other) const {
|
||||
return !operator==(other);
|
||||
}
|
||||
@@ -1,88 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <frc/geometry/Translation3d.h>
|
||||
#include <units/math.h>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Translation3d::Translation3d(units::meter_t x, units::meter_t y,
|
||||
units::meter_t z)
|
||||
: m_x(x), m_y(y), m_z(z) {}
|
||||
|
||||
Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
|
||||
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
|
||||
m_x = rectangular.X();
|
||||
m_y = rectangular.Y();
|
||||
m_z = rectangular.Z();
|
||||
}
|
||||
|
||||
units::meter_t Translation3d::Distance(const Translation3d& other) const {
|
||||
return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
|
||||
units::math::pow<2>(other.m_y - m_y) +
|
||||
units::math::pow<2>(other.m_z - m_z));
|
||||
}
|
||||
|
||||
units::meter_t Translation3d::Norm() const {
|
||||
return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
|
||||
}
|
||||
|
||||
Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
|
||||
Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
|
||||
auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
|
||||
return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
|
||||
units::meter_t{qprime.Z()}};
|
||||
}
|
||||
|
||||
Translation2d Translation3d::ToTranslation2d() const {
|
||||
return Translation2d{m_x, m_y};
|
||||
}
|
||||
|
||||
Translation3d Translation3d::operator+(const Translation3d& other) const {
|
||||
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
|
||||
}
|
||||
|
||||
Translation3d Translation3d::operator-(const Translation3d& other) const {
|
||||
return *this + -other;
|
||||
}
|
||||
|
||||
Translation3d Translation3d::operator-() const { return {-m_x, -m_y, -m_z}; }
|
||||
|
||||
Translation3d Translation3d::operator*(double scalar) const {
|
||||
return {scalar * m_x, scalar * m_y, scalar * m_z};
|
||||
}
|
||||
|
||||
Translation3d Translation3d::operator/(double scalar) const {
|
||||
return *this * (1.0 / scalar);
|
||||
}
|
||||
|
||||
bool Translation3d::operator==(const Translation3d& other) const {
|
||||
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
|
||||
units::math::abs(m_y - other.m_y) < 1E-9_m &&
|
||||
units::math::abs(m_z - other.m_z) < 1E-9_m;
|
||||
}
|
||||
|
||||
bool Translation3d::operator!=(const Translation3d& other) const {
|
||||
return !operator==(other);
|
||||
}
|
||||
@@ -38,13 +38,20 @@ PhotonCamera::PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
|
||||
const std::string& cameraName)
|
||||
: mainTable(instance->GetTable("photonvision")),
|
||||
rootTable(mainTable->GetSubTable(cameraName)),
|
||||
rawBytesEntry(rootTable->GetEntry("rawBytes")),
|
||||
driverModeEntry(rootTable->GetEntry("driverMode")),
|
||||
inputSaveImgEntry(rootTable->GetEntry("inputSaveImgCmd")),
|
||||
outputSaveImgEntry(rootTable->GetEntry("outputSaveImgCmd")),
|
||||
pipelineIndexEntry(rootTable->GetEntry("pipelineIndex")),
|
||||
ledModeEntry(mainTable->GetEntry("ledMode")),
|
||||
versionEntry(mainTable->GetEntry("version")),
|
||||
rawBytesEntry(rootTable->GetRawTopic("rawBytes").Subscribe("raw", {})),
|
||||
driverModeEntry(rootTable->GetBooleanTopic("driverMode").Publish()),
|
||||
inputSaveImgEntry(
|
||||
rootTable->GetBooleanTopic("inputSaveImgCmd").Publish()),
|
||||
outputSaveImgEntry(
|
||||
rootTable->GetBooleanTopic("outputSaveImgCmd").Publish()),
|
||||
pipelineIndexEntry(rootTable->GetIntegerTopic("pipelineIndex").Publish()),
|
||||
ledModeEntry(mainTable->GetIntegerTopic("ledMode").Publish()),
|
||||
versionEntry(mainTable->GetStringTopic("version").Subscribe("")),
|
||||
driverModeSubscriber(
|
||||
rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
|
||||
pipelineIndexSubscriber(
|
||||
rootTable->GetIntegerTopic("pipelineIndex").Subscribe(-1)),
|
||||
ledModeSubscriber(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
|
||||
path(rootTable->GetPath()) {}
|
||||
|
||||
PhotonCamera::PhotonCamera(const std::string& cameraName)
|
||||
@@ -63,13 +70,10 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
|
||||
PhotonPipelineResult result;
|
||||
|
||||
// Fill the packet with latest data and populate result.
|
||||
std::shared_ptr<nt::Value> ntvalue = rawBytesEntry.GetValue();
|
||||
if (!ntvalue) return result;
|
||||
const auto value = rawBytesEntry.Get();
|
||||
if (!value.size()) return result;
|
||||
|
||||
std::string value{ntvalue->GetRaw()};
|
||||
std::vector<char> bytes{value.begin(), value.end()};
|
||||
|
||||
photonlib::Packet packet{bytes};
|
||||
photonlib::Packet packet{value};
|
||||
|
||||
packet >> result;
|
||||
|
||||
@@ -80,31 +84,29 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
|
||||
}
|
||||
|
||||
void PhotonCamera::SetDriverMode(bool driverMode) {
|
||||
driverModeEntry.SetBoolean(driverMode);
|
||||
driverModeEntry.Set(driverMode);
|
||||
}
|
||||
|
||||
void PhotonCamera::TakeInputSnapshot() { inputSaveImgEntry.SetBoolean(true); }
|
||||
void PhotonCamera::TakeInputSnapshot() { inputSaveImgEntry.Set(true); }
|
||||
|
||||
void PhotonCamera::TakeOutputSnapshot() { outputSaveImgEntry.SetBoolean(true); }
|
||||
void PhotonCamera::TakeOutputSnapshot() { outputSaveImgEntry.Set(true); }
|
||||
|
||||
bool PhotonCamera::GetDriverMode() const {
|
||||
return driverModeEntry.GetBoolean(false);
|
||||
}
|
||||
bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); }
|
||||
|
||||
void PhotonCamera::SetPipelineIndex(int index) {
|
||||
pipelineIndexEntry.SetDouble(static_cast<double>(index));
|
||||
pipelineIndexEntry.Set(static_cast<double>(index));
|
||||
}
|
||||
|
||||
int PhotonCamera::GetPipelineIndex() const {
|
||||
return static_cast<int>(pipelineIndexEntry.GetDouble(0));
|
||||
return static_cast<int>(pipelineIndexSubscriber.Get());
|
||||
}
|
||||
|
||||
LEDMode PhotonCamera::GetLEDMode() const {
|
||||
return static_cast<LEDMode>(static_cast<int>(ledModeEntry.GetDouble(-1.0)));
|
||||
return static_cast<LEDMode>(static_cast<int>(ledModeSubscriber.Get()));
|
||||
}
|
||||
|
||||
void PhotonCamera::SetLEDMode(LEDMode mode) {
|
||||
ledModeEntry.SetDouble(static_cast<double>(static_cast<int>(mode)));
|
||||
ledModeEntry.Set(static_cast<double>(static_cast<int>(mode)));
|
||||
}
|
||||
|
||||
void PhotonCamera::VerifyVersion() {
|
||||
@@ -115,7 +117,7 @@ void PhotonCamera::VerifyVersion() {
|
||||
return;
|
||||
this->lastVersionCheckTime = frc::Timer::GetFPGATimestamp();
|
||||
|
||||
const std::string& versionString = versionEntry.GetString("");
|
||||
const std::string& versionString = versionEntry.Get("");
|
||||
if (versionString.empty()) {
|
||||
std::string path_ = path;
|
||||
FRC_ReportError(
|
||||
|
||||
@@ -26,7 +26,7 @@
|
||||
|
||||
namespace photonlib {
|
||||
PhotonPipelineResult::PhotonPipelineResult(
|
||||
units::second_t latency, wpi::span<const PhotonTrackedTarget> targets)
|
||||
units::second_t latency, std::span<const PhotonTrackedTarget> targets)
|
||||
: latency(latency),
|
||||
targets(targets.data(), targets.data() + targets.size()) {}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user