RogiStandard-hal
STM32 向けサークル共通ライブラリ
読み取り中…
検索中…
一致する文字列を見つけられません
Twist2d.hpp
[詳解]
1#pragma once
2#include <units.h>
3#include <units/angular_velocity.h>
4#include <units/velocity.h>
5
6using m_p_s = units::meters_per_second<float>;
7using r_p_s = units::radians_per_second<float>;
8
9class Twist2D {
10 public:
14
15 Twist2D() = default;
16 Twist2D(float x, float y, float theta) : x(x), y(y), theta(theta) {}
17 Twist2D(double x, double y, double theta) : x(x), y(y), theta(theta) {}
19
20 inline Twist2D& operator+=(const Twist2D& rhs) {
21 x += rhs.x;
22 y += rhs.y;
23 theta += rhs.theta;
24 return *this;
25 }
26
27 inline Twist2D& operator-=(const Twist2D& rhs) {
28 x -= rhs.x;
29 y -= rhs.y;
30 theta -= rhs.theta;
31 return *this;
32 }
33
34 inline Twist2D& operator*=(const float& rhs) {
35 x *= rhs;
36 y *= rhs;
37 theta *= rhs;
38 return *this;
39 }
40
41 inline Twist2D& operator/=(const float& rhs) {
42 x /= rhs;
43 y /= rhs;
44 theta /= rhs;
45 return *this;
46 }
47};
48
49inline Twist2D operator+(const Twist2D& lhs, const Twist2D& rhs) {
50 return Twist2D(lhs.x + rhs.x, lhs.y + rhs.y, lhs.theta + rhs.theta);
51}
52inline Twist2D operator-(const Twist2D& lhs, const Twist2D& rhs) {
53 return Twist2D(lhs.x - rhs.x, lhs.y - rhs.y, lhs.theta - rhs.theta);
54}
55inline Twist2D operator*(const Twist2D& lhs, const float& rhs) {
56 return Twist2D(lhs.x * rhs, lhs.y * rhs, lhs.theta * rhs);
57}
58inline Twist2D operator*(const float& lhs, const Twist2D& rhs) {
59 return Twist2D(lhs * rhs.x, lhs * rhs.y, lhs * rhs.theta);
60}
61inline Twist2D operator/(const Twist2D& lhs, const float& rhs) {
62 return Twist2D(lhs.x / rhs, lhs.y / rhs, lhs.theta / rhs);
63}
64inline bool operator==(const Twist2D& lhs, const Twist2D& rhs) {
65 return lhs.x == rhs.x && lhs.y == rhs.y && lhs.theta == rhs.theta;
66}
67inline bool operator!=(const Twist2D& lhs, const Twist2D& rhs) {
68 return lhs.x != rhs.x || lhs.y != rhs.y || lhs.theta != rhs.theta;
69}
Twist2D & operator*=(const float &rhs)
Definition Twist2d.hpp:34
Twist2D(double x, double y, double theta)
Definition Twist2d.hpp:17
Twist2D & operator+=(const Twist2D &rhs)
Definition Twist2d.hpp:20
Twist2D & operator-=(const Twist2D &rhs)
Definition Twist2d.hpp:27
Twist2D & operator/=(const float &rhs)
Definition Twist2d.hpp:41
Twist2D()=default
m_p_s x
Definition Twist2d.hpp:11
m_p_s y
Definition Twist2d.hpp:12
Twist2D(m_p_s x, m_p_s y, r_p_s theta)
Definition Twist2d.hpp:18
Twist2D(float x, float y, float theta)
Definition Twist2d.hpp:16
r_p_s theta
Definition Twist2d.hpp:13
units::meters_per_second< float > m_p_s
Definition Twist2d.hpp:6
bool operator==(const Twist2D &lhs, const Twist2D &rhs)
Definition Twist2d.hpp:64
Twist2D operator+(const Twist2D &lhs, const Twist2D &rhs)
Definition Twist2d.hpp:49
bool operator!=(const Twist2D &lhs, const Twist2D &rhs)
Definition Twist2d.hpp:67
Twist2D operator/(const Twist2D &lhs, const float &rhs)
Definition Twist2d.hpp:61
Twist2D operator*(const Twist2D &lhs, const float &rhs)
Definition Twist2d.hpp:55
Twist2D operator-(const Twist2D &lhs, const Twist2D &rhs)
Definition Twist2d.hpp:52
units::radians_per_second< float > r_p_s
Definition Twist2d.hpp:7
Eigen::Matrix< float, N, 3 > getWheelMatrix(const std::array< WheelPositions, static_cast< std::size_t >(N)> &wheel_positions)