Improving usability of the controller interface

[ARoefer]

In my opinion, the current SymengineController interface is too complicated. It requires users to provide both the free symbols in their expressions, as well as specify exactly which joints to use. There are a couple of problems with this:

1. It requires them to manually keep track of all their free symbols and passing them to the piece of code, initializing the controller. This requires a lot of data passing, which I think is hostile to the idea of modularization.
2. Not sure whether I remember this correctly, but the used joints and the joints referenced in the soft constraints need to coincide. Meaning the user has to keep track of which joints are actually used in their expressions, leading to them having to have a very detailed knowledge of the robot they're using.
3. All the information needed to initialize the controller is already present in the soft constraints.

My suggestion is redesigning the controller initialization to include an algorithm like this one:

def configure_controller(controller, robot, soft_cs, filter=set()):
    free_symbols = set()
    
    # Find free symbols in soft constraints
    for sc in soft_cs.values():
        for f in sc:
            if hasattr(f, 'free_symbols'):
                free_symbols = free_symbols.union(f.free_symbols)
    
    # Select joints referenced in soft constraints which are not supposed to be filtered out
    controller.set_controlled_joints([j for j in robot.get_joint_names() if robot.joint_to_symbol_map[j] in free_symbols and j not in filter])

    # Find additional free symbols in joint constraints
    for jc in controller.joint_constraints.values():
        for f in jc:
            if hasattr(f, 'free_symbols'):
                free_symbols = free_symbols.union(f.free_symbols)
    
    # Find additional free symbols in hard constraints
    for hc in controller.hard_constraints.values():
        for f in hc:
            if hasattr(f, 'free_symbols'):
                free_symbols = free_symbols.union(f.free_symbols)

    # Setting soft constraints and free symbols
    controller.update_soft_constraints(soft_cs, free_symbols)

    return free_symbols

I've used an algorithm like this under the old controller interface in another project of mine (Implementation).
@ichumuh referenced here that the free_symbols call is very slow on large matrices. Personally, I've not had this problem when directly working on the expressions, but I've not profiled it either. This is definitely something to investigate.

[ichumuh]

I'll make the free symbols optional, if I have some time, and check if it is faster to extract them from the constraints instead of the jacobian.

Regarding the controlled joint names:

1. They are provided by the whole body controller, so you don't really have to keep track of the joints.
2. For my robots, the whole body controller never controlled all the joints from the urdf, so I'd always had to filter.
3. I could make this optional as well, though I'd keep the current whitelist approach. A filter requires you to figure out which joints your controller does not control, which is more difficult I belief.

[ARoefer]

Oh, I must've overlooked the whole body controller.
On point 3: I don't quite get how the filter requires me to know which joints are not controlled by it. The way I've always thought about the filter is as a blacklist.
Whether one prefers black- or whitelists is a matter of discussion. Personally, I think blacklists are easier to use, than whitelists, but one could also view a blacklist as a "loose cannon" approach.

[ichumuh]

I don't quite get how the filter requires me to know which joints are not controlled by it.

Isn't this kind of the definition? All the joints in the filter are removed. So if your controller doesn't control the gripper, you have to somehow figure out which joints belong to the gripper. I'd do it by computing the difference between the controlled joints and a list of all joints. This is more work than just handing over the list of joints that my controller controls. So at least for my usecases, a whitelist is more convenient.

[ARoefer]

I think the difference here is really in practical usage. Instead of formulating the filter as a post processing step which removes joints, I'd view it as a pre processing which keeps joints from being added. Either way, as you point out, convenience is dependant on use case

[ARoefer]

Hello, I had a free moment and decided to record the build times for some of my controllers. These are my results:

N Symbols	N Constraints	Time Matrix Extraction	Time Constraint Extraction	Remaining Generation Time
103	20	0.00081s	0.00117s	0.06416s
116	28	0.00196s	0.00206s	0.09415s

So overall extracting the symbols from the matrix directly, seems to be faster than extracting the symbols iteratively, though the gap seems to narrow with a rising number of constraints. I suspect it might be, that free_symbols is more complex on larger objects, while the time for the python iterations does not increase at the same rate.

Given your previous concerns, I assume you use waaaay more constraints than I do, which is why the symbol extraction becomes such an issue?

EDIT:
I realized that there was an easy way for me to add more "sensible" constraints to my test controllers. It turns out that my hunch was correct. Here the results from my new test:

N Symbols	N Constraints	Time Matrix Extraction	Time Constraint Extraction	Remaining Generation Time
173	38	0.01454s	0.00596s	0.46529s
224	56	0.02670s	0.01040s	0.71553s

So it seems that extraction from the constraints is faster once the controller becomes larger.
Tested on a system with an Intel i7-8550U CPU.

[ichumuh]

Seems like .free_symbols is fast now, which resolves this problem. With that speed I can probably remove the input parameter and always call .free_symbols.

[ichumuh]

I finally had some time to look into this. The symengine controller doesn't require free symbols anymore and will use .free_symbols if none are supplied. But this is only fast if symengine is up to date.