Function stepAndCheckLoopClosed

tests/integration/test_Issue719.cpp:159–188

============================================================================== Steps the world and asserts (a) the linkage stays finite the whole time and (b) the closed loop stays closed: the loop-closure anchor on the last link must not drift away from its anchor on the base by more than kMaxLoopClosureError. Without the fix, the Dantzig-only configuration scrubs the singular solution to zero an

// kMaxLoopClosureError. Without the fix, the Dantzig-only configuration scrubs
// the singular solution to zero and the loop drifts open, violating (b).
void stepAndCheckLoopClosed(
    const WorldPtr& world,
    const SkeletonPtr& skel,
    BodyNode* base,
    BodyNode* lastLink,
    const Eigen::Vector3d& loopAnchor)
{
  const Eigen::Vector3d off1
      = lastLink->getWorldTransform().inverse() * loopAnchor;
  const Eigen::Vector3d off2 = base->getWorldTransform().inverse() * loopAnchor;
  const std::size_t numSteps = 100;
  for (std::size_t i = 0; i < numSteps; ++i) {
    // Apply a small actuation/perturbation on the first revolute joint.
    skel->getJoint(1)->setForce(0, 0.01);

    world->step();

    ASSERT_TRUE(skel->getPositions().allFinite())
        << "Non-finite positions at step " << i << ": "
        << skel->getPositions().transpose();
    ASSERT_TRUE(skel->getVelocities().allFinite())
        << "Non-finite velocities at step " << i << ": "
        << skel->getVelocities().transpose();
    const Eigen::Vector3d p1 = lastLink->getWorldTransform() * off1;
    const Eigen::Vector3d p2 = base->getWorldTransform() * off2;
    const double gap = (p1 - p2).norm();
    ASSERT_LT(gap, kMaxLoopClosureError)
        << "Loop drifted open at step " << i << ": gap=" << gap;
  }
}

//==============================================================================
// Exercises PART A: the default World solver is a BoxedLcpConstraintSolver with