hulk/crates/coordinate_systems/src/lib.rs at aed6e86a3daba899da68f50b325fae7796957090 · schmidma/hulk · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
use approx_derive::{AbsDiffEq, RelativeEq};
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};

macro_rules! generate_coordinate_system {
    ($($(#[$doc:meta])* $i:ident),* $(,)?) => {
        $(
            #[derive(
                Clone,
                Copy,
                Debug,
                Default,
                Deserialize,
                Eq,
                Hash,
                PartialEq,
                Serialize,
                RelativeEq,
                AbsDiffEq,
                PathSerialize,
                PathDeserialize,
                PathIntrospect,
                ros_z::Message,
            )]
            #[abs_diff_eq(epsilon_type = f32)]
            $(#[$doc])*
            pub struct $i;
        )*
    }
}

generate_coordinate_system!(
    /// 3D coordinate system centered on the robot.
    ///
    /// Origin: hip of the robot
    /// X axis pointing forward
    /// Y axis pointing left
    Robot,
    /// 2D coordinate system centered on the robot.
    ///
    /// Origin: center between [LeftSole] and [RightSole], projected onto the ground.
    /// X axis pointing forward
    Ground,
    /// 2D odometry frame fixed at the startup pose of one odometry source.
    ///
    /// Origin: robot [Ground] frame when the odometry source starts.
    /// X axis pointing along the startup ground-frame X axis.
    /// Y axis pointing along the startup ground-frame Y axis.
    Odometry,
    /// coordinate system used to express feet positions in the walking engine.
    ///
    /// Origin: below the robot's hip
    /// X axis pointing forward
    /// Y axis pointing left
    Walk,
    /// Represents the coordinate system of the ground frame next to the upcoming support foot.
    ///
    /// The UpcomingSupport frame helps the robot to anticipate and plan the placement of its foot
    /// for the next step.
    ///
    /// This frame is used by the walking engine to plan steps. Its origin is located at the point
    /// where the ground will be once the current step is completed and walking were to stop in the
    /// following step (zero-step). In other words, it is the coordinate frame equal to the ground
    /// frame where the current swing foot is, assuming it becoming support foot this control frame.
    /// Planning always assumes that the current step is ending this very frame to be able to plan
    /// the next, if walking determines that it actually ended this frame.
    ///
    /// Note:
    /// - "Upcoming support foot" refers to the foot that will become the new support foot after
    ///   the current swing foot lands.
    UpcomingSupport,
    /// 2D coordinate system centered on the field, independent of game-controller side.
    ///
    /// Origin: center of the field
    /// X axis pointing towards the home-side opponent goal before any side-dependent flip
    World,
    /// 2D coordinate system centered on the field,
    ///
    /// Origin: center of the field
    /// X axis pointing towards the opponent goal
    Field,
    /// 3D Intrinsic coordinate system of the camera.
    ///
    /// Origin: center of the camera model
    /// X axis pointing right, Y axis pointing down, Z axis pointing forward
    NormalizedDeviceCoordinates,
    /// 3D coordinate system centered on the camera
    Camera,
    /// 2D Coordinate system of the camera image.
    ///
    /// Origin: top left corner of the image
    /// X axis points right
    /// Y axis points down
    Pixel,
    /// 2D Coordinate system of the camera image.
    /// Same as [Pixel] but the dimensions are normalized to (0.0, 1.0) of the image width and height.
    NormalizedPixel,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf
    Head,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf
    Neck,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf
    Torso,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftInnerShoulder,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftOuterShoulder,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftUpperArm,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftForearm,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightInnerShoulder,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightOuterShoulder,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightUpperArm,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightForearm,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftPelvis,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftHip,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftThigh,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftTibia,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftAnkle,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftFoot,
    /// Same as [LeftFoot] but shifted down to the sole.
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    LeftSole,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightPelvis,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightHip,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightThigh,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightTibia,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightAnkle,
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightFoot,
    /// Same as [RightFoot] but shifted down to the sole.
    /// See [official documentation](https://booster.feishu.cn/wiki/AHctwLAYjiyThnkZctIc6iYunmf)
    RightSole,
    /// 2D Coordinate System for Twix Widgets
    ///
    /// Origin: top left corner of the image
    /// X axis points right
    /// Y axis points down
    Screen,
);