WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
CoordinateSystem.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <wpi/SymbolExports.h>
8
10#include "frc/geometry/Pose3d.h"
13
14namespace frc {
15
16/**
17 * A helper class that converts Pose3d objects between different standard
18 * coordinate frames.
19 */
21 public:
22 /**
23 * Constructs a coordinate system with the given cardinal directions for each
24 * axis.
25 *
26 * @param positiveX The cardinal direction of the positive x-axis.
27 * @param positiveY The cardinal direction of the positive y-axis.
28 * @param positiveZ The cardinal direction of the positive z-axis.
29 * @throws std::domain_error if the coordinate system isn't special orthogonal
30 */
31 constexpr CoordinateSystem(const CoordinateAxis& positiveX,
32 const CoordinateAxis& positiveY,
33 const CoordinateAxis& positiveZ) {
34 // Construct a change of basis matrix from the source coordinate system to
35 // the NWU coordinate system. Each column vector in the change of basis
36 // matrix is one of the old basis vectors mapped to its representation in
37 // the new basis.
38 Eigen::Matrix3d R{
39 {positiveX.m_axis(0), positiveY.m_axis(0), positiveZ.m_axis(0)},
40 {positiveX.m_axis(1), positiveY.m_axis(1), positiveZ.m_axis(1)},
41 {positiveX.m_axis(2), positiveY.m_axis(2), positiveZ.m_axis(2)}};
42
43 // The change of basis matrix should be a pure rotation. The Rotation3d
44 // constructor will verify this by checking for special orthogonality.
45 m_rotation = Rotation3d{R};
46 }
47
48 /**
49 * Returns an instance of the North-West-Up (NWU) coordinate system.
50 *
51 * The +X axis is north, the +Y axis is west, and the +Z axis is up.
52 */
53 constexpr static CoordinateSystem NWU() {
54 return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::W(),
55 CoordinateAxis::U()};
56 }
57
58 /**
59 * Returns an instance of the East-Down-North (EDN) coordinate system.
60 *
61 * The +X axis is east, the +Y axis is down, and the +Z axis is north.
62 */
63 constexpr static CoordinateSystem EDN() {
64 return CoordinateSystem{CoordinateAxis::E(), CoordinateAxis::D(),
65 CoordinateAxis::N()};
66 }
67
68 /**
69 * Returns an instance of the NED coordinate system.
70 *
71 * The +X axis is north, the +Y axis is east, and the +Z axis is down.
72 */
73 constexpr static CoordinateSystem NED() {
74 return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::E(),
75 CoordinateAxis::D()};
76 }
77
78 /**
79 * Converts the given translation from one coordinate system to another.
80 *
81 * @param translation The translation to convert.
82 * @param from The coordinate system the translation starts in.
83 * @param to The coordinate system to which to convert.
84 * @return The given translation in the desired coordinate system.
85 */
86 constexpr static Translation3d Convert(const Translation3d& translation,
88 const CoordinateSystem& to) {
89 return translation.RotateBy(from.m_rotation - to.m_rotation);
90 }
91
92 /**
93 * Converts the given rotation from one coordinate system to another.
94 *
95 * @param rotation The rotation to convert.
96 * @param from The coordinate system the rotation starts in.
97 * @param to The coordinate system to which to convert.
98 * @return The given rotation in the desired coordinate system.
99 */
100 constexpr static Rotation3d Convert(const Rotation3d& rotation,
101 const CoordinateSystem& from,
102 const CoordinateSystem& to) {
103 return rotation.RotateBy(from.m_rotation - to.m_rotation);
104 }
105
106 /**
107 * Converts the given pose from one coordinate system to another.
108 *
109 * @param pose The pose to convert.
110 * @param from The coordinate system the pose starts in.
111 * @param to The coordinate system to which to convert.
112 * @return The given pose in the desired coordinate system.
113 */
114 constexpr static Pose3d Convert(const Pose3d& pose,
115 const CoordinateSystem& from,
116 const CoordinateSystem& to) {
117 return Pose3d{Convert(pose.Translation(), from, to),
118 Convert(pose.Rotation(), from, to)};
119 }
120
121 /**
122 * Converts the given transform from one coordinate system to another.
123 *
124 * @param transform The transform to convert.
125 * @param from The coordinate system the transform starts in.
126 * @param to The coordinate system to which to convert.
127 * @return The given transform in the desired coordinate system.
128 */
129 constexpr static Transform3d Convert(const Transform3d& transform,
130 const CoordinateSystem& from,
131 const CoordinateSystem& to) {
132 const auto coordRot = from.m_rotation - to.m_rotation;
133 return Transform3d{
134 Convert(transform.Translation(), from, to),
135 (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
136 }
137
138 private:
139 // Rotation from this coordinate system to NWU coordinate system
140 Rotation3d m_rotation;
141};
142
143} // namespace frc
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
and restrictions which apply to each piece of software is included later in this file and or inside of the individual applicable source files The disclaimer of warranty in the WPILib license above applies to all code in and nothing in any of the other licenses gives permission to use the names of FIRST nor the names of the WPILib contributors to endorse or promote products derived from this software The following pieces of software have additional or alternate and or and nanopb were all modified for use in Google Inc All rights reserved Redistribution and use in source and binary with or without are permitted provided that the following conditions are this list of conditions and the following disclaimer *Redistributions in binary form must reproduce the above copyright this list of conditions and the following disclaimer in the documentation and or other materials provided with the distribution *Neither the name of Google Inc nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS AND ANY EXPRESS OR IMPLIED BUT NOT LIMITED THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY OR CONSEQUENTIAL WHETHER IN STRICT OR EVEN IF ADVISED OF THE POSSIBILITY OF SUCH January AND DISTRIBUTION Definitions License shall mean the terms and conditions for and distribution as defined by Sections through of this document Licensor shall mean the copyright owner or entity authorized by the copyright owner that is granting the License Legal Entity shall mean the union of the acting entity and all other entities that control are controlled by or are under common control with that entity For the purposes of this definition control direct or to cause the direction or management of such whether by contract or including but not limited to software source documentation and configuration files Object form shall mean any form resulting from mechanical transformation or translation of a Source including but not limited to compiled object generated and conversions to other media types Work shall mean the work of whether in Source or Object made available under the as indicated by a copyright notice that is included in or attached to the whether in Source or Object that is based or other modifications as a an original work of authorship For the purposes of this Derivative Works shall not include works that remain separable from
Definition ThirdPartyNotices.txt:141
A class representing a coordinate system axis within the NWU coordinate system.
Definition CoordinateAxis.h:20
A helper class that converts Pose3d objects between different standard coordinate frames.
Definition CoordinateSystem.h:20
static constexpr Pose3d Convert(const Pose3d &pose, const CoordinateSystem &from, const CoordinateSystem &to)
Converts the given pose from one coordinate system to another.
Definition CoordinateSystem.h:114
static constexpr CoordinateSystem NWU()
Returns an instance of the North-West-Up (NWU) coordinate system.
Definition CoordinateSystem.h:53
static constexpr CoordinateSystem EDN()
Returns an instance of the East-Down-North (EDN) coordinate system.
Definition CoordinateSystem.h:63
static constexpr Rotation3d Convert(const Rotation3d &rotation, const CoordinateSystem &from, const CoordinateSystem &to)
Converts the given rotation from one coordinate system to another.
Definition CoordinateSystem.h:100
static constexpr Transform3d Convert(const Transform3d &transform, const CoordinateSystem &from, const CoordinateSystem &to)
Converts the given transform from one coordinate system to another.
Definition CoordinateSystem.h:129
static constexpr Translation3d Convert(const Translation3d &translation, const CoordinateSystem &from, const CoordinateSystem &to)
Converts the given translation from one coordinate system to another.
Definition CoordinateSystem.h:86
constexpr CoordinateSystem(const CoordinateAxis &positiveX, const CoordinateAxis &positiveY, const CoordinateAxis &positiveZ)
Constructs a coordinate system with the given cardinal directions for each axis.
Definition CoordinateSystem.h:31
static constexpr CoordinateSystem NED()
Returns an instance of the NED coordinate system.
Definition CoordinateSystem.h:73
Represents a 3D pose containing translational and rotational elements.
Definition Pose3d.h:28
constexpr const Translation3d & Translation() const
Returns the underlying translation.
Definition Pose3d.h:120
constexpr const Rotation3d & Rotation() const
Returns the underlying rotation.
Definition Pose3d.h:148
A rotation in a 3D coordinate frame represented by a quaternion.
Definition Rotation3d.h:29
constexpr Rotation3d RotateBy(const Rotation3d &other) const
Adds the new rotation to the current rotation.
Definition Rotation3d.h:322
Represents a transformation for a Pose3d in the pose's frame.
Definition Transform3d.h:21
constexpr const Rotation3d & Rotation() const
Returns the rotational component of the transformation.
Definition Transform3d.h:135
constexpr const Translation3d & Translation() const
Returns the translation component of the transformation.
Definition Transform3d.h:94
Represents a translation in 3D space.
Definition Translation3d.h:26
constexpr Translation3d RotateBy(const Rotation3d &other) const
Applies a rotation to the translation in 3D space.
Definition Translation3d.h:144
Definition CAN.h:11