PR2 with DART physics

[osrf-migration]

Original report (archived issue) by Peter Horak (Bitbucket: pchorak).

---

If you load the PR2 model while using DART physics, the model eventually collapses (inf or nan pose values) and the simulation may crash. The problem has to do with the weld constraints added to simulate the kinematic loops in the PR2 (see pull request #2756). Part of the cause is the LCP solver (issue #892). However, even if using the projected Gauss-Seidel solver, the simulation still fails albeit after more time.

The main problem is that some of the weld constraints are set with non-identity relative transforms because an assumption in the code for creating loop joints is violated. Part of the assumption is true: if a link has multiple parent joints, it will not have a free joint parent (which may have an initialized/non-identity transform). However, the pose of the link also depends on any earlier ancestor joints, which may include free joints with initialized transforms.

Sometimes a model only has one kinematic tree (and thus one root/free joint). In this case, all parent joints of the link share that one free joint as an ancestor, so the weld constraint relative transforms are identity as they should be. However, the *_gripper_*_parallel_link links of the PR2 have no parents links, so they are all given free joints initialized with different transforms by the time that weld constraints are created for their shared children (which form kinematic loops). Later, once all the normal joints (revolute, prismatic, etc.) are initialized, the constraints are violated before the simulation even begins.

• The bug could be solved by initializing the free joints with all the other joints instead of when first created. This would require keeping track of the free joints after they are created.

• I believe a more elegant solution would be to explicitly set the relative transforms of any weld constraints to identity. This would ensure that they are set correctly regardless of the poses of body nodes at the time of creation. However, this would require a change to DART (proposed in issue #903).

[osrf-migration]

Original comment by Peter Horak (Bitbucket: pchorak).

---

• Edited issue description

[osrf-migration]

Original comment by Peter Horak (Bitbucket: pchorak).

---

Here is a draft of a solution taking the first approach.