Couple lateral friction constraint magnitudes.

[adamheins]

As has been noted before, PyBullet has an odd behavior where the friction force exerted on an object depends on its orientation. For a small self-contained example, see this gist, in which a box is pushed on the ground plane by an applied force. When the box is pushed along either the x- or y-axis, the resulting friction force is correct. However, when it is oriented and pushed along a 45 deg angle to the axes, we get a lower, incorrect friction force.

It seems to me that the reason for this is that the two friction constraints for each contact point are decoupled (until clipped to the implicit friction cone). This means that they have different effective masses (because effective mass depends on orientation through the constraint Jacobian), such that the resulting impulse is not aligned with the velocity at the contact point. But this doesn't make physical sense: when the contact point is sliding, the friction force should act in the opposite direction to oppose the motion.

This PR is an initial attempt to fix this behavior, by making the effective mass of each pair friction constraints the same. In particular, I compute an effective mass that keeps the kinetic energy the same as when the constraints are decoupled. If one compiles PyBullet with this change and then runs the gist example linked above, one sees that the friction forces are now correct regardless of the orientation. However, I'm not sure if there are other aspects of this problem that I haven't thought about.

Fixing this behavior would be very desirable for facilitating research on planar pushing, so I'm very interested to hear what others think about it

[TrianglesPCT]

Potential issue with using the result of addMultiBodyFrictionConstraint like that, it expands each time you call it, so if during the 2nd call the vector grows it might move itself, make the reference to the first friction invalid.

[adamheins]

Nice catch! I just updated the code to now get the first friction constraint based on its index, rather than the old (potentially invalid) reference.