MaxDistanceConstraint

[anastasiabolotnikova]

I need to implement a maximum distance constraint.

It is essentially a reversed collision constraint, so 99% of the codes is the same.
Previously I tried to “sneak in” the max distance constraint behavior within the CollisionConstraint class by adding a boolean “reverse”, but it starts to look awkward and confusing in the code.

With this PR I propose the addition of the max distance constraint by duplicating collision constraint codes in mc_rtc and Tasks/constr/maxDist and adapting the functions of the MaxDistanceConstraint to get the reversed constraint behavior.

I need a bit of help to finalize and test this implementation. I tried to copy the logic/code/behaviour of CollisionConstraint as follows:

• In mc_rbdyn/RobotModule.h _commonMaxDistances vector is declared.
• In mc_robogami robot module _commonMaxDistances is defined.
• In mc_control/MCController.h maxDistanceConstraint is declared.
• In mc_control/MCController.cpp new MaxDistanceConstraint is created and filled with maxDists using elements from commonMaxDistances().
• In mc_control/mc_halfsitpose_controller.cpp maxDistanceConstraint is added to the qpsolver.
• MaxDistanceConstraint::addToSolverImpl creates an instance of tasks::qp::MaxDistConstr and calls maxdistConstr.addToSolver.

To my best understanding, with all that the constraint should be in the solver and updated on every iteration along with all other tasks and constraints.

However, when I try to test it with mc_robogami/constr/maxDist and a sample controller I don't see the update function of the maxDistanceConstraint being called, and I am not sure why. I'd appreciate to get a feedback on what am I missing?

P.S Eventually, perhaps it could be interesting to have implementation of something general like DistanceConstraint, which could unite CollisionConstraint/MinDistanceConstraint and MaxDistanceConstraint. Because behaviour of something that could be called MinDistanceConstraint might not be always be associated with collision avoidance: e.g. an empty area of a room which robot should not enter (for whatever reason, just an example).

[mergify]

Tick the box to add this pull request to the merge queue (same as @mergifyio queue).

• Queue this pull request

[arntanguy]

Thanks a lot for the contribution and going through the effort of proposing to integrate it within mc_rtc!

Sorry I don't have much time right now for a deep review right now. Some initial thoughts.

It is essentially a reversed collision constraint, so 99% of the codes is the same.
Previously I tried to “sneak in” the max distance constraint behavior within the CollisionConstraint class by adding a boolean “reverse”, but it starts to look awkward and confusing in the code.

This sounded like a good idea. Whenever possible I'd rather go with generalizing the existing code than duplicating it, even if it makes it slightly more complicated. Why did you abandon this route? The specific commit you're referring to here looks reasonable.

As it stands, the duplicated implementation duplicates a lot of things, included some pretty involved GUI monitory code and a lot of logic around the management of the per-body-pair constraints.

P.S Eventually, perhaps it could be interesting to have implementation of something general like DistanceConstraint, which could unite CollisionConstraint/MinDistanceConstraint and MaxDistanceConstraint. Because behaviour of something that could be called MinDistanceConstraint might not be always be associated with collision avoidance: e.g. an empty area of a room which robot should not enter (for whatever reason, just an example).

Yes, I would rather go this way:

• Rename CollisionConstraint to DistanceConstraint
• As far as I see, it should be possible to choose the behaviour between min/max distance per collision pair
• We can modify the loading of collision constraints to include whether this is a min distance/max distance constraint. If this is just a pure reversal, I'm actually wondering if we shouldn't do it per-constraint, keeping min distance as the default. E.g

AddCollisions: # keep for backwards compatibility, rename to AddDistanceConstraint for future use
    - r1: <robot1>
      r2: <robot2>
      collisions:
      - body1: <link_on_robot1>
        body2: <link_on_robot2>
        iDist: 0.05
        sDist: 0.025
        maxDistance: true
      - body1: Link2
        body2: base_link
        iDist: 0.05
        sDist: 0.025
        maxDistance: false # this is a collision aka min distance constraint

with corresponding modification for the C++ struct. Btw what would be the meaning of the interaction/safety distance in that context?

• Since the Task implementation does not look too involved we might as well have it. In general I'd be fine if new features were only implemented in TVM.

Regarding why the update functions do not get called, on first glance I did not see anything obviously wrong. The logic is sound: an update in Convex::Output::Position should cause Update::Value which in turn should cause your updateValue function to be called. That is if something in the graph depends on Output::Value, which it should if you added the task to the solver.

Do you see any blocking point besides implementation to unifying CollisionConstraint and your proposed MaxDistanceConstraint?

[arntanguy]

Is this intended to be used as a global collision (always on) for your robot? If so it might make sense to included it here indeed. If we go with the general DistanceConstraint replacing the CollisionConstraint this becomes unnecessary

[arntanguy]

If we go the DistanceConstraint route this could be absorbed into mc_rbdyn::Collision description (that would probably need renaming with backwards compatibility for clarity)

[anastasiabolotnikova]

Yes, I would rather go this way:

* Rename `CollisionConstraint` to `DistanceConstraint`

Thanks @arntanguy for already taking a quick look into it and giving me feedback! While processing it, I realized that this will already do the job, without having to add a single line of code to mc_rtc...

The only problem with that is the collision vs. distance terminology. If we go the more general DistanceConstr route, we need to ensure backward compatibility with all the legacy "collision" stuff, while also providing access to more general "distance" classes/structures/loaders etc.

To start making things more general, I suggest to go ahead and rename CollisionConstr to DistanceConstr in the Tasks (this alone should not break anything, right?) and adapt mc_rtc to use the new class name. I'll make a new PR with this non-duplicating implementation of a general DistanceConstr.

In general I'd be fine if new features were only implemented in TVM.

I didn't touch the TVM part at all for now. I can take a look at it next week.

Regarding why the update functions do not get called, on first glance I did not see anything obviously wrong. The logic is sound: an update in Convex::Output::Position should cause Update::Value which in turn should cause your updateValue function to be called. That is if something in the graph depends on Output::Value, which it should if you added the task to the solver.

When you have time, I'd appreciate if you could look into it in a bit more details and see if you can explain why update is not called. In case I need to implement something similar in the future, I'd like to understand why this part does not work 🙏