Control Allocator: Metric #24199

Pedro-Roque · 2025-01-10T19:51:54Z

Solved Problem

Adds metric control allocation (ie non-normalized) to make possible model-based control schemes.

Solution

Replicates pseudo-inverse allocator but without normalization of the outputs
Allows usage on control allocation for compatible vehicle types (such as spacecraft sc in https://github.com/DISCOWER/PX4-Space-Systems/tree/dev-sc_ctl_alloc/src/modules/sc_control_allocator )

Changelog Entry

For release notes:

Feature: Metric Control Allocation
New parameter: XYZ_Z
Documentation: Need to clarify control allocation page

Test coverage

Unit/integration test: available
Tested in SITL, will provide logs

Context

Depends on #24196

github-actions · 2025-01-10T22:14:25Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 40 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +40  +0.0%     +40    .text
  +0.4%     +32  +0.4%     +32    ../../src/modules/control_allocator/ControlAllocator.cpp
  +0.4%     +12  +0.4%     +12    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
  -0.1%      -4  -0.1%      -4    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+0.0%     +38  [ = ]       0    .debug_abbrev
  -0.8%     -18  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +8  [ = ]       0    .debug_frame
+0.0%     +31  [ = ]       0    .debug_info
  +0.1%     +35  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
-0.0%     -37  [ = ]       0    .debug_line
  -0.1%      -7  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.4%      -5  [ = ]       0    task/task_cancelpt.c
+0.0%     +72  [ = ]       0    .debug_loc
  +0.7%     +72  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
-0.0%      -6  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +3.1%      +2  [ = ]       0    task/task_cancelpt.c
+0.0%     +98  [ = ]       0    .debug_str
  +0.9%     +87  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
  +0.0%     +11  [ = ]       0    ../../src/modules/control_allocator/ControlAllocator.cpp
-0.3%     -40  [ = ]       0    [Unmapped]
+0.0%    +196  +0.0%     +40    TOTAL

px4_fmu-v6x [Total VM Diff: 40 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +40  +0.0%     +40    .text
  +0.4%     +32  +0.4%     +32    ../../src/modules/control_allocator/ControlAllocator.cpp
  +0.4%     +12  +0.4%     +12    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
  -0.1%      -4  -0.1%      -4    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+0.0%     +38  [ = ]       0    .debug_abbrev
  -0.8%     -18  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +8  [ = ]       0    .debug_frame
+0.0%     +31  [ = ]       0    .debug_info
  +0.1%     +35  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
-0.0%     -29  [ = ]       0    .debug_line
  -0.1%      -7  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.3%      +3  [ = ]       0    task/task_cancelpt.c
+0.0%     +72  [ = ]       0    .debug_loc
  +0.7%     +72  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
-0.0%     -10  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -3.0%      -2  [ = ]       0    task/task_cancelpt.c
+0.0%     +98  [ = ]       0    .debug_str
  +0.9%     +87  [ = ]       0    ../../src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
  +0.0%     +11  [ = ]       0    ../../src/modules/control_allocator/ControlAllocator.cpp
-0.1%     -40  [ = ]       0    [Unmapped]
+0.0%    +200  +0.0%     +40    TOTAL

Updated: 2025-01-17T20:21:13

Jaeyoung-Lim · 2025-01-13T21:30:28Z

src/lib/allocation/control_allocation/ControlAllocationMetric.cpp

+	bool update_normalization_scale)
+{
+	ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators,
+			update_normalization_scale);


Is update_normalization_scale ever used?

Not by metric allocator. It is used here in ControlAllocationPseudoInverse.cpp lines 53 and 62.

The issue is that if this is removed, than the code in

PX4-Autopilot/src/modules/control_allocator/ControlAllocator.cpp

Lines 592 to 593 in 5147394

_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],

config.linearization_point[i], total_num_actuators, reason == EffectivenessUpdateReason::CONFIGURATION_UPDATE);

will break if the metric allocator is used there.

I can, however, remove it to avoid confusion and assume that if the user wishes to use it with control_allocator, there will be trouble. Perhaps that's the correct way?

Pedro-Roque · 2025-01-14T14:28:14Z

@Jaeyoung-Lim I set the parameter to false to make it clear that we don't care about normalization in metric allocator.

Pedro-Roque · 2025-01-14T14:31:22Z

@sfuhrer would be happy to take inputs so we can merge this soon

sfuhrer · 2025-01-15T09:26:37Z

@Pedro-Roque yes sure, happy to have a look. Could you first rebase on latest main, as the PRs this was based on are since in main? And at the same time you could squash your commits. That makes reviewing bit easier. Thanks!

Pedro-Roque · 2025-01-15T11:40:15Z

@sfuhrer done

DronecodeBot · 2025-01-15T17:20:15Z

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-sync-q-a-jan-15-2025/43274/1

sfuhrer

Would be nice to untangle the matrix normalization from the actual allocation algorithm inside the ControlAllocationPseudoInverse. Then the metric allocation wouldn't even specific methods, but it would only be a flag (normalize or not) prior to passing it to the allocator.
If that cannot be done now then I'm okay to get this in, but would rename the files.

sfuhrer · 2025-01-16T17:01:48Z

src/lib/control_allocation/control_allocation/CMakeLists.txt

@@ -34,6 +34,8 @@
 px4_add_library(ControlAllocation
 	ControlAllocation.cpp
 	ControlAllocation.hpp
+	ControlAllocationMetric.cpp


Did you consider calling it ControlAllocationPseudoInverseMetric? That would make it more clear which parts of the existing, normalized, pipeline you're replacing.

Next step would then be to move all the normalization and scaling of the allocation matrix out of the PseudoInverse. The algorithms in the end don't care whether the allocation matrix is normalized or not, do they? Would ControlAllocationSequentialDesaturation work out of the box with a metric allocation matrix?

To address this I've created an internal variable to activate metric allocation. Then we keep the same structure and if non-metric is being used, the allocator behaves as previously expected.

I'd need to look in detail towards the sequential desaturation algorithm, but since I don't use it, I don't have a good answer as of now. In principle, yes, this should be possible.

…ethod

…testing

Pedro-Roque · 2025-01-16T21:41:43Z

Would be nice to untangle the matrix normalization from the actual allocation algorithm inside the ControlAllocationPseudoInverse. Then the metric allocation wouldn't even specific methods, but it would only be a flag (normalize or not) prior to passing it to the allocator. If that cannot be done now then I'm okay to get this in, but would rename the files.

Thanks for the input @sfuhrer ! I've done exactly that for now.

The same interface could be added to the sequential desaturation, but since I don't work with that allocator, it's hard for me to test. But the principle would be the same...

sfuhrer · 2025-01-17T08:58:49Z

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp

@@ -46,6 +46,7 @@
 #pragma once

 #include "ControlAllocation.hpp"
+#include <px4_platform_common/log.h>


I guess this isn't necessary, as you don't enable it due to

``` // adding #include <px4_platform_common/log.h> + PX4_WARN leads to failed linking on test

Removed the requested line. Let's get it merged? 😃

…on) (#24199) * add: metric allocation * add: actual files * rft: moved metric allocation to pseudo-inverse via flag with public method * del: removed metric allocation test and added test in pseudo-inverse testing * rft: deleted extra newline at the end of pseudo inverse test file * feat: removed unnecessary log include

…on) (PX4#24199) * add: metric allocation * add: actual files * rft: moved metric allocation to pseudo-inverse via flag with public method * del: removed metric allocation test and added test in pseudo-inverse testing * rft: deleted extra newline at the end of pseudo inverse test file * feat: removed unnecessary log include

Pedro-Roque requested review from dagar and Jaeyoung-Lim January 10, 2025 19:51

Pedro-Roque changed the title ~~Pr metric allocator library~~ Control Allocator: Overhaul Jan 10, 2025

Pedro-Roque mentioned this pull request Jan 10, 2025

Pr metric allocation #24198

Closed

Jaeyoung-Lim requested a review from sfuhrer January 13, 2025 17:07

Pedro-Roque force-pushed the pr-metric-allocator-library branch from f60507b to b257fe9 Compare January 13, 2025 19:59

Pedro-Roque changed the title ~~Control Allocator: Overhaul~~ Control Allocator: Metric Jan 13, 2025

Pedro-Roque marked this pull request as ready for review January 13, 2025 20:17

Jaeyoung-Lim reviewed Jan 13, 2025

View reviewed changes

Pedro-Roque added 2 commits January 15, 2025 12:36

add: metric allocation

efe070d

add: actual files

0f7f340

Pedro-Roque force-pushed the pr-metric-allocator-library branch from efcca10 to 0f7f340 Compare January 15, 2025 11:39

sfuhrer reviewed Jan 16, 2025

View reviewed changes

Pedro-Roque added 2 commits January 16, 2025 22:37

rft: moved metric allocation to pseudo-inverse via flag with public m…

6d3db2a

…ethod

del: removed metric allocation test and added test in pseudo-inverse …

038c5fb

…testing

rft: deleted extra newline at the end of pseudo inverse test file

837b5e6

sfuhrer reviewed Jan 17, 2025

View reviewed changes

Pedro-Roque added 2 commits January 17, 2025 18:59

feat: removed unnecessary log include

42e3293

Merge branch 'main' into pr-metric-allocator-library

8c25724

sfuhrer approved these changes Jan 20, 2025

View reviewed changes

sfuhrer merged commit b09340c into main Jan 20, 2025
60 of 61 checks passed

sfuhrer deleted the pr-metric-allocator-library branch January 20, 2025 12:30

sfuhrer mentioned this pull request Jan 20, 2025

Spacecraft Build and Bare Control Allocator #24221

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Control Allocator: Metric #24199

Control Allocator: Metric #24199

Pedro-Roque commented Jan 10, 2025 •

edited

Loading

github-actions bot commented Jan 10, 2025 •

edited

Loading

Jaeyoung-Lim Jan 13, 2025

Pedro-Roque Jan 14, 2025 •

edited

Loading

Pedro-Roque Jan 14, 2025 •

edited

Loading

Pedro-Roque commented Jan 14, 2025

Pedro-Roque commented Jan 14, 2025

sfuhrer commented Jan 15, 2025

Pedro-Roque commented Jan 15, 2025

DronecodeBot commented Jan 15, 2025

sfuhrer left a comment •

edited

Loading

sfuhrer Jan 16, 2025

Pedro-Roque Jan 16, 2025

Pedro-Roque commented Jan 16, 2025

sfuhrer Jan 17, 2025

Pedro-Roque Jan 17, 2025

	_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],
	config.linearization_point[i], total_num_actuators, reason == EffectivenessUpdateReason::CONFIGURATION_UPDATE);

Control Allocator: Metric #24199

Control Allocator: Metric #24199

Conversation

Pedro-Roque commented Jan 10, 2025 • edited Loading

Solved Problem

Solution

Changelog Entry

Test coverage

Context

github-actions bot commented Jan 10, 2025 • edited Loading

🔎 FLASH Analysis

Jaeyoung-Lim Jan 13, 2025

Choose a reason for hiding this comment

Pedro-Roque Jan 14, 2025 • edited Loading

Choose a reason for hiding this comment

Pedro-Roque Jan 14, 2025 • edited Loading

Choose a reason for hiding this comment

Pedro-Roque commented Jan 14, 2025

Pedro-Roque commented Jan 14, 2025

sfuhrer commented Jan 15, 2025

Pedro-Roque commented Jan 15, 2025

DronecodeBot commented Jan 15, 2025

sfuhrer left a comment • edited Loading

Choose a reason for hiding this comment

sfuhrer Jan 16, 2025

Choose a reason for hiding this comment

Pedro-Roque Jan 16, 2025

Choose a reason for hiding this comment

Pedro-Roque commented Jan 16, 2025

sfuhrer Jan 17, 2025

Choose a reason for hiding this comment

Pedro-Roque Jan 17, 2025

Choose a reason for hiding this comment

Pedro-Roque commented Jan 10, 2025 •

edited

Loading

github-actions bot commented Jan 10, 2025 •

edited

Loading

Pedro-Roque Jan 14, 2025 •

edited

Loading

Pedro-Roque Jan 14, 2025 •

edited

Loading

sfuhrer left a comment •

edited

Loading