fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 09:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mecanum: fix inverse kinematics

Browse Source
This commit is contained in:
chfriedrich98
2024-12-02 10:54:43 +01:00
committed by chfriedrich98
parent 5dcccd999c
commit 2eda5659eb
1 changed files with 7 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp
+7 -23
View File
@@ -268,31 +268,15 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_spe
float lateral_speed_normalized, float lateral_speed_normalized,
float speed_diff) float speed_diff)
{ {
// Prioritize ratio between forward and lateral speed over either magnitude const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf(speed_diff);
float combined_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized);
if (combined_speed > 1.f) { if (total_speed > 1.f) { // Adjust speed setpoints if infeasible
forward_speed_normalized /= combined_speed; const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized));
lateral_speed_normalized /= combined_speed; const float magnitude = (1.f - fabsf(speed_diff)) / (sinf(theta) + cosf(theta));
combined_speed = 1.f; const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f)));
} forward_speed_normalized *= magnitude * normalization;
lateral_speed_normalized *= magnitude * normalization;
// Prioritize yaw rate over forward and lateral speed
const float total_speed = combined_speed + fabsf(speed_diff);
if (total_speed > 1.f) {
const float excess_velocity = fabsf(total_speed - 1.f);
const float forward_throttle_temp = forward_speed_normalized - sign(forward_speed_normalized) * 0.5f * excess_velocity;
const float lateral_throttle_temp = lateral_speed_normalized - sign(lateral_speed_normalized) * 0.5f * excess_velocity;
if (sign(forward_throttle_temp) == sign(forward_speed_normalized)
&& sign(lateral_speed_normalized) == sign(lateral_throttle_temp)) {
forward_speed_normalized = forward_throttle_temp;
lateral_speed_normalized = lateral_throttle_temp;
} else {
forward_speed_normalized = lateral_speed_normalized = 0.f;
}
} }
// Calculate motor commands // Calculate motor commands