Skip to content

Commit 849ef18

Browse files
AndrewyxAndrewyx
committed
euclid change
1 parent 1bd2cd9 commit 849ef18

2 files changed

Lines changed: 188 additions & 101 deletions

File tree

‎src/software/physics/euclidean_to_wheel.cpp‎

Lines changed: 106 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -8,69 +8,144 @@
88
#include "software/geom/angular_velocity.h"
99
#include "software/geom/vector.h"
1010

11+
#ifdef DEBUG_WHEEL
1112
EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants)
1213
: robot_constants_(robot_constants)
1314
{
1415
// Angles [rads]
15-
const double p = robot_constants_.front_wheel_angle_deg * M_PI / 180.;
16-
const double t = robot_constants_.back_wheel_angle_deg * M_PI / 180.;
17-
18-
// Beta is angle of wheel rolling direction relative to X-axis
19-
// LOOK AT (software)/src/shared/robot_constants.h to see X-axis
20-
const double b_fr = -(M_PI - p);
21-
const double b_fl = -p;
22-
const double b_bl = t;
23-
const double b_br = (M_PI - t);
24-
25-
// Mapped to the robot frame: +X = Forward, +Y = Left
26-
const double fr_x = robot_constants_.fr_x_pos_meters;
27-
const double fr_y = robot_constants_.fr_y_pos_meters;
28-
const double fl_x = robot_constants_.fl_x_pos_meters;
29-
const double fl_y = robot_constants_.fl_y_pos_meters;
30-
const double bl_x = robot_constants_.bl_x_pos_meters;
31-
const double bl_y = robot_constants_.bl_y_pos_meters;
32-
const double br_x = robot_constants_.br_x_pos_meters;
33-
const double br_y = robot_constants_.br_y_pos_meters;
34-
35-
36-
// Assuming that CCW when looking end of shaft into motor is the positive direction.
37-
// Formula is u_1 = v_x * cos(B_1) + v_y * sin(B_1) + W * (x_1 * sin(B_1) - y_1 *
38-
// cos(B_1))
16+
auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.;
17+
auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.;
18+
19+
auto cos_p = std::cos(p);
20+
auto sin_p = std::sin(p);
21+
auto cos_t = std::cos(t);
22+
auto sin_t = std::sin(t);
23+
24+
// 1. Physical wheel coordinates in meters
25+
// Mapped to the robot frame: +X = Right, +Y = Forward
26+
double fr_x = 0.06632, fr_y = 0.03485;
27+
double br_x = 0.05592, br_y = -0.04985;
28+
29+
// 2. Unit drive vectors (extracted directly from the matrix's linear columns)
30+
double fr_dx = -sin_p, fr_dy = cos_p;
31+
double br_dx = sin_t, br_dy = cos_t;
32+
33+
// 3. Dynamically calculate the rotational lever arms using the 2D cross product
34+
// Lever Arm = | X * Dy - Y * Dx |
35+
double rot_f = std::abs((fr_x * fr_dy) - (fr_y * fr_dx));
36+
double rot_b = std::abs((br_x * br_dy) - (br_y * br_dx));
37+
38+
// clang-format off
39+
euclidean_to_wheel_velocity_D_ <<
40+
-sin_p, cos_p, rot_f,
41+
-sin_p, -cos_p, rot_f,
42+
sin_t, -cos_t, rot_b,
43+
sin_t, cos_t, rot_b;
44+
// clang-format on
45+
46+
// Calculate Pseudo-inverse dynamically
47+
wheel_to_euclidean_velocity_D_inverse_ =
48+
(euclidean_to_wheel_velocity_D_.transpose() * euclidean_to_wheel_velocity_D_)
49+
.inverse() *
50+
euclidean_to_wheel_velocity_D_.transpose();
51+
}
52+
53+
#else
54+
55+
EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants)
56+
: robot_constants_(robot_constants)
57+
{
58+
// Phi, the angle between the hemisphere line of the robot and the front wheel axles
59+
// [rads]
60+
auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.;
61+
62+
// Theta, the angle between the hemisphere line of the robot and the rear wheel axles
63+
// [rads]
64+
auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.;
65+
66+
// Caching repeated calculations
67+
auto cos_p = std::cos(p);
68+
auto cos_t = std::cos(t);
69+
auto sin_p = std::sin(p);
70+
auto sin_t = std::sin(t);
3971

4072
// clang-format off
4173
euclidean_to_wheel_velocity_D_ <<
42-
std::cos(b_fr), std::sin(b_fr), (fr_x * std::sin(b_fr) - fr_y * std::cos(b_fr)),
43-
std::cos(b_fl), std::sin(b_fl), (fl_x * std::sin(b_fl) - fl_y * std::cos(b_fl)),
44-
std::cos(b_bl), std::sin(b_bl), (bl_x * std::sin(b_bl) - bl_y * std::cos(b_bl)),
45-
std::cos(b_br), std::sin(b_br), (br_x * std::sin(b_br) - br_y * std::cos(b_br));
74+
-sin_p, cos_p, 1,
75+
-sin_p, -cos_p, 1,
76+
sin_t, -cos_t, 1,
77+
sin_t, cos_t, 1;
4678
// clang-format on
4779

4880
// Inverse of euclidean to wheel matrix (D) for converting wheel velocity to euclidean
4981
// velocity.
5082
// NOTE: The pseudoinverse given in the paper
5183
// (http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 17) is incorrect. The inverse
5284
// was calculated using Wolfram Alpha: https://bit.ly/3A08BJX
85+
auto i = 1 / (2 * sin_p + 2 * sin_t);
5386

54-
// Calculate Pseudo-inverse dynamically
87+
auto j_denom = 2 * cos_p * cos_p + 2 * cos_t * cos_t;
88+
auto j1 = cos_p / j_denom;
89+
auto j2 = cos_t / j_denom;
5590

56-
wheel_to_euclidean_velocity_D_inverse_ =
57-
euclidean_to_wheel_velocity_D_.completeOrthogonalDecomposition().pseudoInverse();
91+
auto k_denom = 2 * sin_p + 2 * sin_t;
92+
auto k1 = sin_t / k_denom;
93+
auto k2 = sin_p / k_denom;
94+
95+
// clang-format off
96+
wheel_to_euclidean_velocity_D_inverse_ <<
97+
-i, -i, i, i,
98+
j1, -j1, -j2, j2,
99+
k1, k1, k2, k2;
100+
// clang-format on
58101
}
59102

103+
#endif
104+
105+
#ifdef DEBUG_WHEEL
60106
WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const
61107
{
62108
// The generalized D matrix natively handles the w -> tangential velocity
63109
// conversion because its third column already represents the lever arm in meters.
64110
return euclidean_to_wheel_velocity_D_ * euclidean_velocity;
65111
}
66112

113+
67114
EuclideanSpace_t EuclideanToWheel::getEuclideanVelocity(
68115
const WheelSpace_t& wheel_velocity) const
69116
{
70117
// The D inverse matrix natively outputs w in rad/s.
71118
return wheel_to_euclidean_velocity_D_inverse_ * wheel_velocity;
72119
}
73120

121+
#else
122+
123+
WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const
124+
{
125+
// need to multiply the angular velocity by the robot radius to
126+
// calculate the wheel velocity (robot tangential velocity)
127+
// ref: http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 8
128+
euclidean_velocity[2] = euclidean_velocity[2] * robot_constants_.robot_radius_m;
129+
130+
return euclidean_to_wheel_velocity_D_ * euclidean_velocity;
131+
}
132+
133+
EuclideanSpace_t EuclideanToWheel::getEuclideanVelocity(
134+
const WheelSpace_t& wheel_velocity) const
135+
{
136+
EuclideanSpace_t euclidean_velocity =
137+
wheel_to_euclidean_velocity_D_inverse_ * wheel_velocity;
138+
139+
// The above multiplication will calculate the tangential robot
140+
// velocity. This can be divided by the robot radius to calculate
141+
// the angular velocity
142+
// ref: http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 8
143+
euclidean_velocity[2] = euclidean_velocity[2] / robot_constants_.robot_radius_m;
144+
145+
return euclidean_velocity;
146+
}
147+
#endif
148+
74149
WheelSpace_t EuclideanToWheel::rampWheelVelocity(
75150
const WheelSpace_t& current_wheel_velocity, const WheelSpace_t& target_wheel_velocity,
76151
const double& time_to_ramp)

0 commit comments

Comments
 (0)