Fix grouping of contact constraints

[azeey]

Description copied from gazebo-forks#6:

DART crashes when simulating a modest number of objects that come into contact with each other.

To test, run the following file in ign-gazebo (ign gazebo -v 4 cube5.sdf.txt)
cube5.sdf.txt (courtesy of @mjcarroll)

Before the PR, ign-gazebo crashes with

ODE INTERNAL ERROR 1: assertion "bNormalizationResult" failed in _dNormalize4() [odemath.h:42]

With the PR:

For a given set of contact constraints, the constraint solver first creates a grouping of related constraints, and then solves each group independently. There seems to be a problem with this grouping and some constraints end up being in two groups.

The ContactConstraint::uniteSkeleton function unites two skeletons associated with a given contact constraint. It does that by setting the mUnionRootSkeleton member of one of the skeletons to point to the current root skeleton of the second skeleton. To decide which of the two skeletons' root node becomes the new union's root node, the size of the skeletons' current unions are compared.

As an example, let A,B,C, and D be skeletons associated with 3 contact constraints. When ContactConstraint::uniteSkeleton is called for constraints 1 and 2, skeleton B is chosen to be the root node.

1. A, B = A -> B
2. C, A = C -> B // Since the root of A is B, C's mUnionRootSkeleton now points to B
3. D, B = B -> D // Let's assume D is chosen because D's union size is larger.

When D is chosen for constraint 3, A and C are not updated. Thus, later on when ContactConstraing::getRootSkeleton is called on constraint 1 or 2, B is returned instead of D. This results in error in the grouping of constraints.

It's not clear to me why this error causes a crash, but the hint that led me to a solution is that I tried disabling the grouping altogether and letting all the constraints be solved in a single call to ConstraingSolver::solveConstrainedGroup and that didn't crash. My guess is that when a constraint ends up in two groups, a solution for that constraint is computed twice and the resulting impulses are erroneously accumulated.

For additional clarity, I instrumented the code to output a dot file and generated graphs of the constraints and their grouping. For the attached SDF file, here is an example grouping for frame 413

The ellipses represent the skeletons (cubes and ground_plane) and the edges represent a contact constraint between two skeletons. The light grey edges are constraints where one of the skeletons is a static body. The rectangles represent the constraint groupings. With proper grouping, each node in the graph should only have an edge to another node inside its group (rectangle). The exception is for static/immobile skeletons which are allowed to have constraints across multiple groups because impulses are not applied on them. But notice how model_3_0_3 has a constraint with a model_3_0_4, a skeleton outside its group. I believe in this example, the group labeled model_2_3_4 and model_3_0_0 should have been one group.

In contrast, here is frame 413 after the fix. Granted, the collisions in this frame are going to be different from the one run without the fix, but I wanted to show that this PR does not disable grouping:

---

Before creating a pull request

• Document new methods and classes
• Format new code files using clang-format

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md
• Add unit test(s) for this change

[jslee02]

This is awesome! Thank you for catching this out and the detailed analysis.

It seems ConstraintBase::getRootSkeleton() should be always used for other constraint classes. Let me work on that.