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
10using 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]
26template <int N>
27Eigen::Matrix<float, N, 3> getWheelMatrix(
28 const std::array<WheelPositions, static_cast<std::size_t>(N)>&
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
43 static_cast<float>(2.0 * M_PI * wheel_radius.value());
45 }
46
47 return matrix;
48}
units::meters< float > meters
Definition Pose2d.hpp:6
Eigen::Matrix< float, N, 3 > getWheelMatrix(const std::array< WheelPositions, static_cast< std::size_t >(N)> &wheel_positions)