RogiStandard-hal
STM32 向けサークル共通ライブラリ
読み取り中…
検索中…
一致する文字列を見つけられません
WheelMatrix.hpp
[詳解]
1
#pragma once
2
3
#include <units/length.h>
4
5
#include <Eigen/Dense>
6
#include <cmath>
7
8
#include "
Wheel.hpp
"
9
10
using
meters
= units::meters<float>;
11
12
// 機体速度から各車輪の回転速度(RPS)を求めるための変換行列を計算する
13
// 参考: https://kra.kibe.la/notes/8128#2-入力からホイール出力まで逆運動学
14
//
15
// 計算式:
16
// [ v_1 ] [ cos(θ_1) sin(θ_1) L_1 ] [ v_x ]
17
// [ v_2 ] = (1/2πr) * [ cos(θ_2) sin(θ_2) L_2 ] * [ v_y ]
18
// [ ... ] [ ... ... ... ] [ ω ]
19
// [ v_N ] [ cos(θ_N) sin(θ_N) L_N ]
20
//
21
// - v_i : 第i車輪の目標回転速度 [rev/s]
22
// - r : 車輪半径 [m]
23
// - L_i : 重心から車輪への回転モーメント距離成分 (x*sinθ - y*cosθ)
24
// - v_x, v_y : 機体の並進速度 [m/s]
25
// - ω : 機体の角速度 [rad/s]
26
template
<
int
N>
27
Eigen::Matrix<float, N, 3>
getWheelMatrix
(
28
const
std::array<
WheelPositions
,
static_cast<
std::size_t
>
(
N
)>&
29
wheel_positions
) {
30
Eigen::Matrix<float, N, 3>
matrix
;
31
32
for
(
int
i
= 0;
i
<
N
;
i
++) {
33
const
WheelPositions
&
wp
=
wheel_positions
[
static_cast<
std::size_t
>
(
i
)];
34
Pose2D
wheel_pos
=
wp
.position;
35
meters
wheel_radius
=
wp
.radius;
36
37
matrix
(
i
, 0) =
static_cast<
float
>
(
cos
(
wheel_pos
.theta.value()));
38
matrix
(
i
, 1) =
static_cast<
float
>
(
sin
(
wheel_pos
.theta.value()));
39
matrix
(
i
, 2) = (
wheel_pos
.x.value() *
matrix
(
i
, 1) -
40
wheel_pos
.y.value() *
matrix
(
i
, 0));
41
42
float
wheel_circumference
=
43
static_cast<
float
>
(2.0 *
M_PI
*
wheel_radius
.value());
44
matrix
.row(
i
) /=
wheel_circumference
;
45
}
46
47
return
matrix
;
48
}
Wheel.hpp
Pose2D
Definition
Pose2d.hpp:9
meters
units::meters< float > meters
Definition
Pose2d.hpp:6
getWheelMatrix
Eigen::Matrix< float, N, 3 > getWheelMatrix(const std::array< WheelPositions, static_cast< std::size_t >(N)> &wheel_positions)
Definition
WheelMatrix.hpp:27
WheelPositions
Definition
Wheel.hpp:12
Utils
WheelMatrix
Inc
WheelMatrix.hpp
構築:
1.9.8