Prefer to use the active controller if multiple controllers apply

[stephanie-eng]

Description

Resolves #1631

Currently, if controller a is active , nractive[a] will be 1. If it is inactive, nractive[a] will be 0. Assuming controller a is active (nractive[a] = 1) and controller b is inactive (nractive[b] = 0), we would follow the second condition (nractive[a] > nractive[b])and return false. This means that preference is given to inactive controllers.

In practice, if two controllers A and B are able to control the same number of joints and only controller A is active, TrajectoryExecutionManager will select the inactive controller B first and activate it. For a subsequent move, we select the now-inactive controller A, and use that, switching between controller A and B for each move.

This change prioritizes the active controller first, such that if controller A is active, it will be used for all subsequent moves until it is deactivated.

[sjahr]

You did not create them but can we replace these horrible variable names with something verbose? What does nr mean 🙈 ?

[codecov]

Codecov Report

Patch coverage has no change and project coverage change: +0.02 🎉

Comparison is base (31bcfef) 50.50% compared to head (8b4d275) 50.51%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2251      +/-   ##
==========================================
+ Coverage   50.50%   50.51%   +0.02%     
==========================================
  Files         386      386              
  Lines       31732    31732              
==========================================
+ Hits        16022    16026       +4     
+ Misses      15710    15706       -4     

see 2 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

[sjahr]

After talking to @stephanie-eng I'd say we merge this fix and then do a bigger refactoring of this code

[sea-bass]

The plan sounds good...

I was trying to read through it and have no idea what's going on, haha.