contact-6D: unclear - could be pulling force ?
Closed this issue · 6 comments
Hello, thanks for your nice work in TSID. I am working on the examples and I have a doubt regarding the 6D contact.
Compared with the contact-point, I understand that you are linearizing the friction cone which is leading to four conditions (l.78 - l.82) to check plus additionally, you need to check whether the contact force is pointing in the right direction (l.84). This is why matrix B has 5 lines in this case.
For the contact-6D, you are adding 4 contact points, that's why the matrix B contains 4 blocks (4x3) of the linearized friction cone (l.105). What I don't understand is the last line of the matrix B, which is just a horizontal stack of the 4 contact normals (l.108 - l.111).
In my eyes, this can lead to a problem: we are comparing if the sum of the contact forces is pointing in the right direction, but not every single one of them, right? I would have expected to have 4 additional lines, one for each of the contact points, instead of 1 line that should account for all of them. This would lead to a matrix B with 4x5=20 lines instead of 4x4+1=17 lines.
I would be very happy if you can help me to understand your choice or the point that I'm missing currently. Thanks a lot.
Hello @fabinsch!
Let me try to answer with my little bit of experience with TSID which I really enjoy.
Maybe the point you are missing here is the concept of "force generator matrix". The way of modeling 6D planar contact as a stack of four point contacts is straightforward but redundant. In that way you will get the 4 points x 3D forces = 12D constraint representation.
But you are modeling the 6D contact expecting to get the 3D force + 3D torque at the contact point = 6D constraint.
The problem is that eiQuadProg lying underneath the TSID can't handle the redundant constraints. So the force generator matrix is a small hack to mitigate this doing a transform from 12D to 6D cutting of the redundancy, that's why the B matrix has less size than you expect.
And the resulting 3D force is constrained to be positive so no pulling force is allowed.
You can check how Andrea explains this concept by himself in his great "TSID Implementation" lecture and slides from here.
And the resulting 3D force is constrained to be positive so no pulling force is allowed.
In fact, pulling seems to be allowed because having
In fact, pulling seems to be allowed because having f1+f2+f3+f4≥0, does not enforce the fact that fi≥0
@jcarpent you are absolutely right, strictly speaking the individual forces in vertices of the 6D contact patch can be negative at some vertices and only the sum of them can't, so maybe I answered not exactly the formulation that @fabinsch asked.
What I tried to emphasize above is that the resultant force in 6D contact is constrained to be positive, and as an "end-user" of TSID I will see only this value as the result of contact-6D simulation. So I wouldn't expect that the robot with 6D contact can pull itself by the leg towards the ground, even knowing that somewhere deeply inside the optimization formulation there are some negative-signed force at the contact patch vertex needed to generate the appropriate torque at the contact point etc.
The reason you have only 1 constraint on the normal force (rather than 4) is because: i) the linearized friction cone constraints are already preventing the forces on each vertex to be pulling, and ii) TSID allows you to specify a minimum net normal force for each 6D contact (this is a design choice, but I find it very reasonable). So in the end you'll have all forces that are pushing, and the net normal force that is above the user-defined threshold fMin.
Ok. Sounds good to me.
Alright, thanks a lot for your explanations @andreadelprete and @egordv .