@@ -1362,15 +1362,45 @@ representations of the constraint Jacobian and related matrices.
13621362 axes. The right panel illustrates our solution to this problem. We still update one contact at a time, but within a
13631363 contact we update along non-orthogonal axes adapted to the constraint surface, as follows. First, we optimize the
13641364 quadratic cost along the ray from the tip of the cone through the current solution. Then we slice the cone with a
1365- hyperplane passing through the current solution and orthogonal to the contact normal. This yields an ellipsoid - which
1366- can be up to 5-dimensional given our contact model. Now we optimize the quadratic cost within this ellipsoid. This is
1367- an instance of quadratically constrained quadratic programming (QCQP). Since there is only one scalar constraint
1365+ hyperplane passing through the current solution and orthogonal to the contact normal. This yields an ellipsoid which
1366+ can be up to 5-dimensional, given our contact model. Now we optimize the quadratic cost within this ellipsoid. This
1367+ is an instance of quadratically constrained quadratic programming (QCQP). Since there is only one scalar constraint
13681368 (however nonlinear it may be), the dual is a scalar optimization problem over the unknown Lagrange multiplier. We
13691369 solve this problem with Newton's method applied until convergence -- which in practice takes less than 10 iterations,
13701370 and involves small matrices. Overall this algorithm has similar behavior to PGS for pyramidal cones, but it can
13711371 handle elliptic cones without approximating them. It does more work per contact, however the contact dimensionality
13721372 is smaller, and these two factors roughly balance each other.
13731373
1374+ .. _soIsland :
1375+
1376+ Constraint islands
1377+ ~~~~~~~~~~~~~~~~~~
1378+
1379+ .. image :: ../images/computation/island.svg
1380+ :width: 58%
1381+ :align: right
1382+
1383+ Consider the abstract graph defined by degrees of freedom (dofs) and constraints. A vertex is all the dofs in a single
1384+ kinematic subtree; an edge is a constraint (a contact or equality) between two bodies belonging to different subtrees. A
1385+ *constraint island * is a disjoint sub-graph which can be solved for independently, because constraint forces cannot
1386+ propagate between islands. Constraint island discovery and construction ("islanding") invloves finding the disjoint
1387+ subgraphs and reordering both the dofs and constraints to make them memory-contiguous. This amounts to a
1388+ block-diagonalization of the constraint Jacobian :math: `J`, as illustrated in the figure. On the left is the monolithic
1389+ Jacobian of size :math: `\nc \times \nv `, where we use the :ref: `corresponding <Framework >` size names from MuJoCo's data
1390+ structures ``mjData.nefc `` and ``mjModel.nv ``. On the right is the block-diagonalized Jacobian with 3 islands that can
1391+ be solved indepenently. Note that islanding also identifies unconstrained dofs, so ``mjData.nidof ``, the total number of
1392+ dofs in all islands, might be smaller than ``mjModel.nv ``. While islanding is not free (see implementation in
1393+ `engine_island.c <https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_island.c >`__), it is worth the
1394+ effort:
1395+
1396+ - Different islands require different numbers of iterations to converge, and a monolithic solve would run for the
1397+ number required by the slowest island.
1398+ - Unconstrained dofs are completely untouched by the solver, which otherwise needs to discover that they are unaffected.
1399+ - Solving separate islands can be multi-threaded.
1400+
1401+ Islanding is not yet supported by the PGS solver.
1402+
1403+
13741404.. _soParameters :
13751405
13761406Parameters
@@ -1694,7 +1724,8 @@ The stages below compute quantities that depend on the generalized positions ``m
169417247. Compute the sparse factorization of the joint-space inertia matrix: :ref: `mj_factorM `
169517258. Construct the list of active contacts. This includes both broad-phase and near-phase collision detection:
16961726 :ref: `mj_collision `
1697- 9. Construct the constraint Jacobian and compute the constraint residuals: :ref: `mj_makeConstraint `
1727+ 9. Construct the constraint Jacobian, compute the constraint residuals, construct islands: :ref: `mj_makeConstraint `,
1728+ ``mj_island `` (not yet exposed in the API)
1698172910. Compute the actuator lengths and moment arms: :ref: `mj_transmission `
1699173011. Compute the matrices and vectors needed by the constraint solvers: :ref: `mj_projectConstraint `
1700173112. Compute sensor data that only depends on position, and the potential energy if enabled: :ref: `mj_sensorPos `,
0 commit comments