Skip to content

Commit 34f22ef

Browse files
Ryan4253Ryan4253
committed
Make SE(2) classes use AU
1 parent a8d883c commit 34f22ef

10 files changed

Lines changed: 272 additions & 444 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -144,9 +144,9 @@ add_executable(RaidZeroLib
144144
# include/RaidZeroLib/api/Filter/SlewRate.hpp
145145
include/RaidZeroLib/api/Geometry/Point.hpp
146146
include/RaidZeroLib/api/Geometry/Rotation.hpp
147-
# include/RaidZeroLib/api/Geometry/Transform.hpp
148-
# include/RaidZeroLib/api/Geometry/Twist.hpp
149-
# include/RaidZeroLib/api/Geometry/Pose.hpp
147+
include/RaidZeroLib/api/Geometry/Transform.hpp
148+
include/RaidZeroLib/api/Geometry/Twist.hpp
149+
include/RaidZeroLib/api/Geometry/Pose.hpp
150150
# include/RaidZeroLib/api/Units/Units.hpp
151151
# include/RaidZeroLib/api.hpp
152152

@@ -164,20 +164,20 @@ add_executable(RaidZeroLib
164164
# src/RaidZeroLib/api/Pathing/CubicBezier.cpp
165165
# src/RaidZeroLib/api/Pathing/PiecewiseCubicBezier.cpp
166166
# src/RaidZeroLib/api/Filter/SlewRate.cpp
167-
# src/RaidZeroLib/api/Geometry/Twist.cpp
167+
src/RaidZeroLib/api/Geometry/Twist.cpp
168168
src/RaidZeroLib/api/Geometry/Point.cpp
169-
# src/RaidZeroLib/api/Geometry/Transform.cpp
169+
src/RaidZeroLib/api/Geometry/Transform.cpp
170170
src/RaidZeroLib/api/Geometry/Rotation.cpp
171-
# src/RaidZeroLib/api/Geometry/Pose.cpp
171+
src/RaidZeroLib/api/Geometry/Pose.cpp
172172

173173
# test/test/Control/Feedforward/SimpleMotorFeedforwardTest.cpp
174174
# test/test/Control/Iterative/IterativeVelBangBangControllerTest.cpp
175175

176176
test/test/Geometry/RotationTest.cpp
177177
test/test/Geometry/PointTest.cpp
178-
# test/test/Geometry/TwistTest.cpp
179-
# test/test/Geometry/TransformTest.cpp
180-
# test/test/Geometry/PoseTest.cpp
178+
test/test/Geometry/TwistTest.cpp
179+
test/test/Geometry/TransformTest.cpp
180+
test/test/Geometry/PoseTest.cpp
181181

182182
# test/test/Pathing/DiscretePathTest.cpp
183183
# test/test/Pathing/ParametricPathTest.cpp

‎include/RaidZeroLib/api/Geometry/Pose.hpp‎

Lines changed: 24 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -1,63 +1,53 @@
11
#pragma once
22
#include "RaidZeroLib/api/Geometry/Point.hpp"
3-
#include "RaidZeroLib/api/Geometry/Transform.hpp"
4-
#include "RaidZeroLib/api/Geometry/Twist.hpp"
5-
#include "RaidZeroLib/api/Units/Units.hpp"
6-
#include "okapi/api/odometry/odomState.hpp"
3+
#include "RaidZeroLib/api/Geometry/Rotation.hpp"
4+
5+
namespace okapi {
6+
class OdomState;
7+
} // namespace okapi
78

89
namespace rz {
9-
using namespace okapi;
10+
1011
class Transform;
12+
class Twist;
1113

1214
class Pose {
1315
public:
14-
constexpr Pose() = default;
15-
16-
Pose(const Translation& iTranslation, const Rotation& iRotation);
17-
18-
Pose(QLength iX, QLength iY, const Rotation& iRotation);
19-
20-
Pose(const OdomState& iState);
21-
22-
~Pose() = default;
23-
24-
const Translation& getTranslation() const;
25-
26-
const Rotation& getRotation() const;
16+
constexpr Pose() noexcept = default;
2717

28-
QLength X() const;
18+
Pose(const Point& point, const Rotation& rotation) noexcept;
2919

30-
QLength Y() const;
20+
Pose(au::QuantityD<au::Meters> x, au::QuantityD<au::Meters> y, const Rotation& rotation) noexcept;
3121

32-
QAngle Theta() const;
22+
Pose(au::QuantityD<au::Meters> x, au::QuantityD<au::Meters> y, au::QuantityD<au::Radians> angle) noexcept;
3323

34-
Pose operator+(const Transform& rhs) const;
24+
Pose(const okapi::OdomState& state) noexcept;
3525

36-
Transform operator-(const Pose& rhs) const;
26+
const Point& getPoint() const noexcept;
3727

38-
Pose operator*(double scalar) const;
28+
const Rotation& getRotation() const noexcept;
3929

40-
Pose operator/(double scalar) const;
30+
au::QuantityD<au::Meters> X() const noexcept;
4131

42-
bool operator==(const Pose& rhs) const;
32+
au::QuantityD<au::Meters> Y() const noexcept;
4333

44-
bool operator!=(const Pose& rhs) const;
34+
au::QuantityD<au::Radians> Theta() const noexcept;
4535

46-
void operator=(const Pose& rhs);
36+
Pose transformBy(const Transform& rhs) const noexcept;
4737

48-
Pose transformBy(const Transform& rhs) const;
38+
Transform relativeTo(const Pose& rhs) const noexcept;
4939

50-
Pose relativeTo(const Pose& rhs) const;
40+
Pose exp(const Twist& rhs) const noexcept;
5141

52-
Pose exp(const Twist& rhs) const;
42+
Twist log(const Pose& rhs) const noexcept;
5343

54-
Twist log(const Pose& rhs) const;
44+
bool isApprox(const Pose& rhs) const noexcept;
5545

5646
private:
57-
Translation translation;
47+
Point point;
5848
Rotation rotation;
5949
};
6050

61-
QCurvature curvatureToPoint(const Pose& position, const Point& point);
51+
au::QuantityD<au::Inverse<au::Meters>> curvatureToPoint(const Pose& pose, const Point& point) noexcept;
6252

6353
} // namespace rz

‎include/RaidZeroLib/api/Geometry/Transform.hpp‎

Lines changed: 14 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -1,47 +1,37 @@
11
#pragma once
22
#include "RaidZeroLib/api/Geometry/Point.hpp"
3-
#include "RaidZeroLib/api/Geometry/Pose.hpp"
3+
#include "RaidZeroLib/api/Geometry/Rotation.hpp"
44

55
namespace rz {
6+
67
class Pose;
7-
using namespace okapi;
88

99
class Transform {
1010
public:
11-
constexpr Transform() = default;
12-
13-
Transform(const Pose& iInitial, const Pose& iFinal);
14-
15-
Transform(const Translation& iTranslation, const Rotation& iRotation);
16-
17-
~Transform() = default;
18-
19-
const Translation& getTranslation() const;
20-
21-
const Rotation& getRotation() const;
11+
constexpr Transform() noexcept = default;
2212

23-
QLength X() const;
13+
Transform(const Pose& initial, const Pose& final) noexcept;
2414

25-
QLength Y() const;
15+
Transform(const Point& point, const Rotation& rotation) noexcept;
2616

27-
QAngle Theta() const;
17+
const Point& getPoint() const noexcept;
2818

29-
Transform operator+(const Transform& rhs) const;
19+
const Rotation& getRotation() const noexcept;
3020

31-
Transform operator*(double scalar) const;
21+
au::QuantityD<au::Meters> X() const noexcept;
3222

33-
Transform operator/(double scalar) const;
23+
au::QuantityD<au::Meters> Y() const noexcept;
3424

35-
bool operator==(const Transform& rhs) const;
25+
au::QuantityD<au::Radians> Theta() const noexcept;
3626

37-
bool operator!=(const Transform& rhs) const;
27+
Transform operator+(const Transform& rhs) const noexcept;
3828

39-
void operator=(const Transform& rhs);
29+
Transform inverse() const noexcept;
4030

41-
Transform inverse() const;
31+
bool isApprox(const Transform& rhs) const noexcept;
4232

4333
private:
44-
Translation translation;
34+
Point point;
4535
Rotation rotation;
4636
};
4737

‎include/RaidZeroLib/api/Geometry/Twist.hpp‎

Lines changed: 9 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,34 +1,24 @@
11
#pragma once
2-
#include "okapi/api/units/QAngle.hpp"
3-
#include "okapi/api/units/QLength.hpp"
2+
#include "au/au.hpp"
43

54
namespace rz {
6-
using namespace okapi;
75

86
class Twist {
97
public:
10-
Twist(QLength dX, QLength dY, QAngle dTheta);
8+
Twist(au::QuantityD<au::Meters> dx, au::QuantityD<au::Meters> dy, au::QuantityD<au::Radians> dtheta) noexcept;
119

12-
Twist(const Twist& rhs);
10+
au::QuantityD<au::Meters> dX() const noexcept;
1311

14-
~Twist() = default;
12+
au::QuantityD<au::Meters> dY() const noexcept;
1513

16-
QLength dX() const;
14+
au::QuantityD<au::Radians> dTheta() const noexcept;
1715

18-
QLength dY() const;
19-
20-
QAngle dTheta() const;
21-
22-
bool operator==(const Twist& rhs) const;
23-
24-
bool operator!=(const Twist& rhs) const;
25-
26-
void operator=(const Twist& rhs);
16+
bool isApprox(const Twist& rhs) const noexcept;
2717

2818
private:
29-
QLength dx{0.0};
30-
QLength dy{0.0};
31-
QAngle dtheta{0.0};
19+
au::QuantityD<au::Meters> dx = au::ZERO;
20+
au::QuantityD<au::Meters> dy = au::ZERO;
21+
au::QuantityD<au::Radians> dtheta = au::ZERO;
3222
};
3323

3424
} // namespace rz

0 commit comments

Comments
 (0)