Drake
ConstraintSolver< T > Class Template Reference

Solves constraint problems for constraint forces. More...

#include <drake/multibody/constraint/constraint_solver.h>

## Classes

struct  MlcpToLcpData
Structure used to convert a mixed linear complementarity problem to a pure linear complementarity problem (by solving for free variables). More...

## Public Member Functions

ConstraintSolver ()=default

void SolveConstraintProblem (const ConstraintAccelProblemData< T > &problem_data, VectorX< T > *cf) const
Solves the appropriate constraint problem at the acceleration level. More...

Does not allow copy, move, or assignment
ConstraintSolver (const ConstraintSolver &)=delete

ConstraintSolveroperator= (const ConstraintSolver &)=delete

ConstraintSolver (ConstraintSolver &&)=delete

ConstraintSolveroperator= (ConstraintSolver &&)=delete

## Static Public Member Functions

static void ComputeGeneralizedForceFromConstraintForces (const ConstraintAccelProblemData< T > &problem_data, const VectorX< T > &cf, VectorX< T > *generalized_force)
Computes the generalized force on the system from the constraint forces given in packed storage. More...

static void ComputeGeneralizedForceFromConstraintForces (const ConstraintVelProblemData< T > &problem_data, const VectorX< T > &cf, VectorX< T > *generalized_force)
Computes the generalized force on the system from the constraint forces given in packed storage. More...

static void ComputeGeneralizedAcceleration (const ConstraintAccelProblemData< T > &problem_data, const VectorX< T > &cf, VectorX< T > *generalized_acceleration)
Computes the system generalized acceleration due to both external forces and constraint forces. More...

static void ComputeGeneralizedAcceleration (const ConstraintVelProblemData< T > &problem_data, const VectorX< T > &v, const VectorX< T > &cf, double dt, VectorX< T > *generalized_acceleration)
Computes a first-order approximation of generalized acceleration due only to constraint forces. More...

static void ComputeGeneralizedAccelerationFromConstraintForces (const ConstraintAccelProblemData< T > &problem_data, const VectorX< T > &cf, VectorX< T > *generalized_acceleration)
Computes the system generalized acceleration due only to constraint forces. More...

static void ComputeGeneralizedAccelerationFromConstraintForces (const ConstraintVelProblemData< T > &problem_data, const VectorX< T > &cf, VectorX< T > *generalized_acceleration)
Computes the system generalized acceleration due only to constraint forces. More...

static void ComputeGeneralizedVelocityChange (const ConstraintVelProblemData< T > &problem_data, const VectorX< T > &cf, VectorX< T > *generalized_delta_v)
Computes the change to the system generalized velocity from constraint impulses. More...

static void CalcContactForcesInContactFrames (const VectorX< T > &cf, const ConstraintAccelProblemData< T > &problem_data, const std::vector< Matrix2< T >> &contact_frames, std::vector< Vector2< T >> *contact_forces)
Gets the contact forces expressed in each contact frame for 2D contact problems from the "packed" solution returned by SolveConstraintProblem(). More...

static void CalcContactForcesInContactFrames (const VectorX< T > &cf, const ConstraintVelProblemData< T > &problem_data, const std::vector< Matrix2< T >> &contact_frames, std::vector< Vector2< T >> *contact_forces)
Gets the contact forces expressed in each contact frame for 2D contact problems from a "packed" solution returned by, e.g., SolveImpactProblem(). More...

## Velocity-level constraint problems formulated as MLCPs.

Constraint problems can be posed as mixed linear complementarity problems (MLCP), which are problems that take the form:

(a)    Au + X₁y + a = 0
(b)  X₂u + X₃y + x₄ ≥ 0
(c)               y ≥ 0
(d) vᵀ(x₄ + X₂u + X₃y) = 0


where u are "free" variables, y are constrained variables, A, X₁, X₂, and X₃ are given matrices (we label only the most important variables without subscripts to make them stand out) and a and x₄ are given vectors. If A is nonsingular, u can be solved for:

(e) u = -A⁻¹ (a + X₁y)


allowing the mixed LCP to be converted to a "pure" LCP (qq, MM) by:

(f) qq = x₄ - X₂A⁻¹a
(g) MM = X₃ - X₂A⁻¹X₁


This pure LCP can then be solved for y such that:

(h)     MMv + qq ≥ 0
(i)            y ≥ 0
(j) yᵀ(MMv + qq) = 0


and this value for y can be substituted into (e) to obtain the value for u.

### An MLCP-based impact model:

Consider the following problem formulation of a multibody dynamics impact model (taken from [Anitescu 1997]). In a simulator, one could use this model when a collision is detected in order to compute an instantaneous change in velocity.

(1) | M  -Gᵀ  -Nᵀ  -Dᵀ  0  -Lᵀ | | v⁺ | + |-Mv⁻ | = | 0  |
| G   0    0    0   0   0  | | fG | + |  kᴳ | = | 0  |
| N   0    0    0   0   0  | | fN | + |  kᴺ | = | x₅ |
| D   0    0    0   E   0  | | fD | + |  kᴰ | = | x₆ |
| 0   0    μ   -Eᵀ  0   0  | |  λ | + |   0 | = | x₇ |
| L   0    0    0   0   0  | | fL | + |  kᴸ | = | x₈ |
(2) 0 ≤ fN  ⊥  x₅ ≥ 0
(3) 0 ≤ fD  ⊥  x₆ ≥ 0
(4) 0 ≤ λ   ⊥  x₇ ≥ 0
(5) 0 ≤ fL  ⊥  x₈ ≥ 0


Here, the velocity variables v⁻ ∈ ℝⁿᵛ, v ∈ ℝⁿᵛ⁺ correspond to the velocity of the system before and after impulses are applied, respectively. More details will be forthcoming but key variables are M ∈ ℝⁿᵛˣⁿᵛ, the generalized inertia matrix; G ∈ ℝⁿᵇˣⁿᵛ, N ∈ ℝⁿᶜˣⁿᵛ, D ∈ ℝⁿᶜᵏˣⁿᵛ, and L ∈ ℝⁿᵘˣⁿᵛ correspond to Jacobian matrices for various constraints (joints, contact, friction, generic unilateral constraints, respectively); μ ∈ ℝⁿᶜˣⁿᶜ is a diagonal matrix comprised of Coulomb friction coefficients; E ∈ ℝⁿᶜᵏˣⁿᶜ is a binary matrix used to linearize the friction cone (necessary to make this into a linear complementarity problem); fG ∈ ℝⁿᵇ, fN ∈ ℝⁿᶜ, fD ∈ ℝⁿᶜᵏ, and fL ∈ ℝⁿᵘ are constraint impulses; λ ∈ ℝⁿᶜ, x₅, x₆, x₇, and x₈ can be viewed as mathematical programming "slack" variables; and kᴳ ∈ ℝⁿᵇ, kᴺ ∈ ℝⁿᶜ, kᴰ ∈ ℝⁿᶜᵏ, kᴸ ∈ ℝⁿᵘ allow customizing the problem to, e.g., correct constraint violations and simulate restitution. See Variable definitions for complete definitions of nv, nc, nb, etc.

From the notation above in Equations (a)-(d), we can convert the MLCP to a "pure" linear complementarity problem (LCP), which is easier to solve, at least for active-set-type mathematical programming approaches:

 A ≡ | M  -Ĝᵀ|   a ≡ |-Mv⁻ |   X₁ ≡ |-Nᵀ  -Dᵀ  0  -Lᵀ |
| Ĝ   0 |       |  kᴳ |        | 0    0   0   0  |
X₂ ≡ | N   0 |   b ≡ |  kᴺ |   X₃ ≡ | 0    0   0   0  |
| D   0 |       |  kᴰ |        | 0    0   E   0  |
| 0   0 |       |  0  |        | μ   -Eᵀ  0   0  |
| L   0 |       |  kᴸ |        | 0    0   0   0  |
 u ≡ | v⁺ |      y ≡ | fN  |
| fG |          | fD  |
|  λ  |
| fL  |


Where applicable, ConstraintSolver computes solutions to linear equations with rank-deficient A (e.g., AX₅ = x₆) using a least squares approach (the complete orthogonal factorization). Now, using Equations (f) and (g) and defining C as the nv × nv-dimensional upper left block of A⁻¹ (nv is the dimension of the generalized velocities) the pure LCP (qq,MM) is defined as:

MM ≡ | NCNᵀ  NCDᵀ   0   NCLᵀ |
| DCNᵀ  DCDᵀ   E   DCLᵀ |
| μ      -Eᵀ   0   0    |
| LCNᵀ  LCDᵀ   0   LCLᵀ |
qq ≡ | kᴺ - |N 0ⁿᵛ⁺ⁿᵇ|A⁻¹a |
| kᴰ - |D 0ⁿᵛ⁺ⁿᵇ|A⁻¹a |
|       0             |
| kᴸ - |L 0ⁿᵛ⁺ⁿᵇ|A⁻¹a |


where nb is the number of bilateral constraint equations. The solution y will then take the form:

y ≡ | fN |
| fD |
| λ  |
| fL |


The key variables for using the MLCP-based formulations are the matrix A and vector a, as seen in documentation of MlcpToLcpData and the following methods. During its operation, ConstructBaseDiscretizedTimeLcp() constructs (and returns) functions for solving AX=B, where B is a given matrix and X is an unknown matrix. UpdateDiscretizedTimeLcp() computes and returns a during its operation.

### Another use of the MLCP formulation (discretized multi-body dynamics with contact and friction):

Without reconstructing the entire MLCP, we now show a very similar formulation to solve the problem of discretizing the multi-body dynamics equations with contact and friction. This particular formulation provides several nice features: 1) the formulation is semi-implicit and models compliant contact efficiently, including both sticking contact and contact between very stiff surfaces; 2) all constraint forces are computed in Newtons (typical "time stepping methods" require considerable care to correctly compare constraint forces, which are impulsive, and non-constraint forces, which are non-impulsive); and 3) can be made almost symplectic by choosing a representation and computational coordinate frame that minimize velocity-dependent forces (thereby explaining the extreme stability of software like ODE and Bullet that computes dynamics in body frames (minimizing the magnitudes of velocity-dependent forces) and provides the ability to disable gyroscopic forces.

The discretization problem replaces the meaning of v⁻ and v⁺ in the MLCP to mean the generalized velocity at time t and the generalized velocity at time t+h, respectively, for discretization quantum h (or, equivalently, integration step size h). The LCP is adjusted to the form:

MM ≡ | hNCNᵀ+γᴺ  hNCDᵀ   0   hNCLᵀ    |
| hDCNᵀ     hDCDᵀ   E   hDCLᵀ    |
| μ         -Eᵀ     0   0        |
| hLCNᵀ     hLCDᵀ   0   hLCLᵀ+γᴸ |
qq ≡ | kᴺ - |N 0ⁿᵛ⁺ⁿᵇ|A⁻¹a |
| kᴰ - |D 0ⁿᵛ⁺ⁿᵇ|A⁻¹a |
|       0             |
| kᴸ - |L 0ⁿᵛ⁺ⁿᵇ|A⁻¹a |

where γᴺ, γᴸ, kᴺ, kᴸ, and a are all functions of h; documentation that describes how to update these (and dependent) problem data to attain desired constraint stiffness and dissipation is forthcoming.

The procedure one uses to formulate and solve this discretization problem is:

1. Call ConstructBaseDiscretizedTimeLcp()
2. Select an integration step size, dt
3. Compute kᴺ' and kᴸ in the problem data, accounting for dt as necessary
4. Call UpdateDiscretizedTimeLcp(), obtaining MM and qq that encode the linear complementarity problem
5. Solve the linear complementarity problem
6. If LCP solved, quit.
7. Reduce dt and repeat the process from 3. until success.

The solution to the LCP can be used to obtain the constraint forces via PopulatePackedConstraintForcesFromLcpSolution().

### Obtaining the generalized constraint forces:

Given the constraint forces, which have been obtained either through SolveImpactProblem() (in which case the forces are impulsive) or through direct solution of the LCP corresponding to the discretized multibody dynamics problem, followed by PopulatePackedConstraintForcesFromLcpSolution() (in which cases the forces are non-impulsive), the generalized forces/impulses due to the constraints can then be acquired via ComputeGeneralizedForceFromConstraintForces().

void SolveImpactProblem (const ConstraintVelProblemData< T > &problem_data, VectorX< T > *cf) const
Solves the impact problem described above. More...

static void ConstructBaseDiscretizedTimeLcp (const ConstraintVelProblemData< T > &problem_data, MlcpToLcpData *mlcp_to_lcp_data, MatrixX< T > *MM, VectorX< T > *qq)
Computes the base time-discretization of the system using the problem data, resulting in the MM and qq described in velocity-level-MLCPs; if MM and qq are modified no further, the LCP corresponds to an impact problem (i.e., the multibody dynamics problem would not be discretized). More...

static void UpdateDiscretizedTimeLcp (const ConstraintVelProblemData< T > &problem_data, double h, MlcpToLcpData *mlcp_to_lcp_data, VectorX< T > *a, MatrixX< T > *MM, VectorX< T > *qq)
Updates the time-discretization of the LCP initially computed in ConstructBaseDiscretizedTimeLcp() using the problem data and time step h. More...

static void PopulatePackedConstraintForcesFromLcpSolution (const ConstraintVelProblemData< T > &problem_data, const MlcpToLcpData &mlcp_to_lcp_data, const VectorX< T > &zz, const VectorX< T > &a, double dt, VectorX< T > *cf)
Populates the packed constraint force vector from the solution to the linear complementarity problem (LCP) constructed using ConstructBaseDiscretizedTimeLcp() and UpdateDiscretizedTimeLcp(). More...

## Detailed Description

### template<typename T> class drake::multibody::constraint::ConstraintSolver< T >

Solves constraint problems for constraint forces.

Specifically, given problem data corresponding to a rigid or multi-body system constrained bilaterally and/or unilaterally and acted upon by friction, this class computes the constraint forces.

This problem can be formulated as a mixed linear complementarity problem (MLCP)- for 2D problems with Coulomb friction or 3D problems without Coulomb friction- or a mixed complementarity problem (for 3D problems with Coulomb friction). We use a polygonal approximation (of selectable accuracy) to the friction cone, which yields a MLCP in all cases.

Existing algorithms for solving MLCPs, which are based upon algorithms for solving "pure" linear complementarity problems (LCPs), solve smaller classes of problems than the corresponding LCP versions. For example, Lemke's Algorithm, which is provably able to solve the impacting problems covered by this class, can solve LCPs with copositive matrices [Cottle 1992] but MLCPs with only positive semi-definite matrices (the latter is a strict subset of the former) [Sargent 1978].

Rather than using one of these MLCP algorithms, we instead transform the problem into a pure LCP by first solving for the bilateral constraint forces. This method yields an implication of which the user should be aware. Bilateral constraint forces are computed before unilateral constraint forces: the constraint forces will not be evenly distributed between bilateral and unilateral constraints (assuming such a distribution were even possible).

For the normal case of unilateral constraints admitting degrees of freedom, the solution methods in this class support "softening" of the constraints, as described in [Lacoursiere 2007] via the constraint force mixing (CFM) and error reduction parameter (ERP) parameters that are now ubiquitous in game multi-body dynamics simulation libraries.

• [Cottle 1992] R. W. Cottle, J.-S. Pang, and R. E. Stone. The Linear Complementarity Problem. SIAM Classics in Applied Mathematics, 1992.
• [Judice 1992] J. J. Judice, J. Machado, and A. Faustino. An extension of the Lemke's method for the solution of a generalized linear complementarity problem. In System Modeling and Optimization (Lecture Notes in Control and Information Sciences), Springer-Verlag, 1992.
• [Lacoursiere 2007] C. Lacoursiere. Ghosts and Machines: Regularized Variational Methods for Interactive Simulations of Multibodies with Dry Frictional Contacts. Ph. D. thesis (Umea University), 2007.
• [Sargent 1978] R. W. H. Sargent. An efficient implementation of the Lemke Algorithm and its extension to deal with upper and lower bounds. Mathematical Programming Study, 7, 1978.
Template Parameters
 T The vector element type, which must be a valid Eigen scalar.

Instantiated templates for the following scalar types T are provided:

• double They are already available to link against in the containing library.

## Constructor & Destructor Documentation

 ConstraintSolver ( )
default
 ConstraintSolver ( const ConstraintSolver< T > & )
delete
 ConstraintSolver ( ConstraintSolver< T > && )
delete

## Member Function Documentation

 void CalcContactForcesInContactFrames ( const VectorX< T > & cf, const ConstraintAccelProblemData< T > & problem_data, const std::vector< Matrix2< T >> & contact_frames, std::vector< Vector2< T >> * contact_forces )
static

Gets the contact forces expressed in each contact frame for 2D contact problems from the "packed" solution returned by SolveConstraintProblem().

Parameters
 cf the output from SolveConstraintProblem() problem_data the problem data input to SolveConstraintProblem() contact_frames the contact frames corresponding to the contacts. The first column of each matrix should give the contact normal, while the second column gives a contact tangent. For sliding contacts, the contact tangent should point along the direction of sliding. For non-sliding contacts, the tangent direction should be that used to determine problem_data.F. All vectors should be expressed in the global frame. [out] contact_forces a non-null vector of a doublet of values, where the iᵗʰ element represents the force along each basis vector in the iᵗʰ contact frame.
Exceptions
 std::logic_error if contact_forces is null, if contact_forces is not empty, if cf is not the proper size, if the number of tangent directions is not one per non-sliding contact (indicating that the contact problem might not be 2D), if the number of contact frames is not equal to the number of contacts, or if a contact frame does not appear to be orthonormal.
Note
On return, the contact force at the iᵗʰ contact point expressed in the world frame is contact_frames[i] * contact_forces[i].

Here is the call graph for this function:

Here is the caller graph for this function:

 void CalcContactForcesInContactFrames ( const VectorX< T > & cf, const ConstraintVelProblemData< T > & problem_data, const std::vector< Matrix2< T >> & contact_frames, std::vector< Vector2< T >> * contact_forces )
static

Gets the contact forces expressed in each contact frame for 2D contact problems from a "packed" solution returned by, e.g., SolveImpactProblem().

If the constraint forces are impulsive, the contact forces are impulsive (with units of Ns); similarly, if the constraint forces are non-impulsive, the contact forces will be non-impulsive (with units of N).

Parameters
 cf the constraint forces in packed format. problem_data the problem data input to SolveImpactProblem() contact_frames the contact frames corresponding to the contacts. The first column of each matrix should give the contact normal, while the second column gives a contact tangent (specifically, the tangent direction used to determine problem_data.F). All vectors should be expressed in the global frame. [out] contact_forces a non-null vector of a doublet of values, where the iᵗʰ element represents the force along each basis vector in the iᵗʰ contact frame.
Exceptions
 std::logic_error if contact_forces is null, if contact_forces is not empty, if cf is not the proper size, if the number of tangent directions is not one per contact (indicating that the contact problem might not be 2D), if the number of contact frames is not equal to the number of contacts, or if a contact frame does not appear to be orthonormal.
Note
On return, the contact force at the iᵗʰ contact point expressed in the world frame is contact_frames[i] * contact_forces[i].

Here is the call graph for this function:

 static void ComputeGeneralizedAcceleration ( const ConstraintAccelProblemData< T > & problem_data, const VectorX< T > & cf, VectorX< T > * generalized_acceleration )
inlinestatic

Computes the system generalized acceleration due to both external forces and constraint forces.

Parameters
 problem_data The acceleration-level constraint data. cf The computed constraint forces, in the packed storage format described in documentation for SolveConstraintProblem. [out] generalized_acceleration The generalized acceleration, on return.
Exceptions
 std::logic_error if generalized_acceleration is null or cf vector is incorrectly sized.

Here is the caller graph for this function:

 void ComputeGeneralizedAcceleration ( const ConstraintVelProblemData< T > & problem_data, const VectorX< T > & v, const VectorX< T > & cf, double dt, VectorX< T > * generalized_acceleration )
static

Computes a first-order approximation of generalized acceleration due only to constraint forces.

Parameters
 problem_data The velocity-level constraint data. cf The computed constraint forces, in the packed storage format described in documentation for SolveConstraintProblem. v The system generalized velocity at time t. dt The discretization time constant (i.e., the "time step" for simulations) used to take the system's generalized velocities from time t to time t + dt. [out] generalized_acceleration The generalized acceleration, on return. The original will be resized (if necessary) and overwritten.
Warning
This method uses the method problem_data.solve_inertia() in order to compute v(t+dt), so the computational demands may be significant.
Exceptions
 std::logic_error if generalized_acceleration is null or cf vector is incorrectly sized.
Precondition
dt is positive.

Here is the call graph for this function:

 void ComputeGeneralizedAccelerationFromConstraintForces ( const ConstraintAccelProblemData< T > & problem_data, const VectorX< T > & cf, VectorX< T > * generalized_acceleration )
static

Computes the system generalized acceleration due only to constraint forces.

Parameters
 cf The computed constraint forces, in the packed storage format described in documentation for SolveConstraintProblem.
Exceptions
 std::logic_error if generalized_acceleration is null or cf vector is incorrectly sized.

Here is the call graph for this function:

Here is the caller graph for this function:

 void ComputeGeneralizedAccelerationFromConstraintForces ( const ConstraintVelProblemData< T > & problem_data, const VectorX< T > & cf, VectorX< T > * generalized_acceleration )
static

Computes the system generalized acceleration due only to constraint forces.

Parameters
 cf The computed constraint forces, in the packed storage format described in documentation for SolveConstraintProblem.
Exceptions
 std::logic_error if generalized_acceleration is null or cf vector is incorrectly sized.

Here is the call graph for this function:

 void ComputeGeneralizedForceFromConstraintForces ( const ConstraintAccelProblemData< T > & problem_data, const VectorX< T > & cf, VectorX< T > * generalized_force )
static

Computes the generalized force on the system from the constraint forces given in packed storage.

Parameters
 problem_data The data used to compute the contact forces. cf The computed constraint forces, in the packed storage format described in documentation for SolveConstraintProblem. [out] generalized_force The generalized force acting on the system from the total constraint wrench is stored here, on return. This method will resize generalized_force as necessary. The indices of generalized_force will exactly match the indices of problem_data.f.
Exceptions
 std::logic_error if generalized_force is null or cf vector is incorrectly sized.

Get the normal and non-sliding contact forces.

Get the limit forces.

Compute the generalized force.

Here is the caller graph for this function:

 void ComputeGeneralizedForceFromConstraintForces ( const ConstraintVelProblemData< T > & problem_data, const VectorX< T > & cf, VectorX< T > * generalized_force )
static

Computes the generalized force on the system from the constraint forces given in packed storage.

Parameters
 problem_data The data used to compute the contact forces. cf The computed constraint forces, in the packed storage format described in documentation for PopulatePackedConstraintForcesFromLcpSolution(). [out] generalized_force The generalized force acting on the system from the total constraint wrench is stored here, on return. This method will resize generalized_force as necessary. The indices of generalized_force will exactly match the indices of problem_data.f.
Exceptions
 std::logic_error if generalized_force is null or cf vector is incorrectly sized.

Get the normal and tangential contact forces.

Get the limit forces.

Compute the generalized forces.

 void ComputeGeneralizedVelocityChange ( const ConstraintVelProblemData< T > & problem_data, const VectorX< T > & cf, VectorX< T > * generalized_delta_v )
static

Computes the change to the system generalized velocity from constraint impulses.

Parameters
 cf The computed constraint impulses, in the packed storage format described in documentation for SolveImpactProblem.
Exceptions
 std::logic_error if generalized_delta_v is null or cf vector is incorrectly sized.

Here is the call graph for this function:

Here is the caller graph for this function:

 void ConstructBaseDiscretizedTimeLcp ( const ConstraintVelProblemData< T > & problem_data, MlcpToLcpData * mlcp_to_lcp_data, MatrixX< T > * MM, VectorX< T > * qq )
static

Computes the base time-discretization of the system using the problem data, resulting in the MM and qq described in velocity-level-MLCPs; if MM and qq are modified no further, the LCP corresponds to an impact problem (i.e., the multibody dynamics problem would not be discretized).

The data output (mlcp_to_lcp_data, MM, and qq) can be updated using a particular time step in UpdateDiscretizedTimeLcp(), resulting in a non-impulsive problem formulation. In that case, the multibody dynamics equations are discretized, as described in UpdateDiscretizedTimeLcp().

Note
If you really do wish to solve an impact problem, you should use SolveImpactProblem() instead.
Parameters
 problem_data the constraint problem data. [out] mlcp_to_lcp_data a pointer to a valid MlcpToLcpData object; the caller must ensure that this pointer remains valid through the constraint solution process. [out] MM a pointer to a matrix that will contain the parts of the LCP matrix not dependent upon the time step on return. [out] qq a pointer to a vector that will contain the parts of the LCP vector not dependent upon the time step on return.
Precondition
mlcp_to_lcp_data, MM, and qq are non-null on entry.
UpdateDiscretizedTimeLcp()
 ConstraintSolver& operator= ( ConstraintSolver< T > && )
delete
 ConstraintSolver& operator= ( const ConstraintSolver< T > & )
delete
 void PopulatePackedConstraintForcesFromLcpSolution ( const ConstraintVelProblemData< T > & problem_data, const MlcpToLcpData & mlcp_to_lcp_data, const VectorX< T > & zz, const VectorX< T > & a, double dt, VectorX< T > * cf )
static

Populates the packed constraint force vector from the solution to the linear complementarity problem (LCP) constructed using ConstructBaseDiscretizedTimeLcp() and UpdateDiscretizedTimeLcp().

Parameters
 problem_data the constraint problem data. mlcp_to_lcp_data a reference to a MlcpToLcpData object. zz the solution to the LCP resulting from UpdateDiscretizedTimeLcp(). a the vector a output from UpdateDiscretizedTimeLcp(). dt the time step used to discretize the problem. [out] cf the constraint forces, on return. The first nc elements of cf correspond to the magnitudes of the contact forces applied along the normals of the nc contact points. The next elements of cf correspond to the frictional forces along the r spanning directions at each point of contact. The first r values (after the initial nc elements) correspond to the first contact, the next r values correspond to the second contact, etc. The next ℓ values of cf correspond to the impulsive forces applied to enforce unilateral constraint functions. The final b values of cf correspond to the forces applied to enforce generic bilateral constraints. This packed storage format can be turned into more useful representations through ComputeGeneralizedForceFromConstraintForces() and CalcContactForcesInContactFrames().
Precondition
cf is non-null.

Here is the call graph for this function:

Here is the caller graph for this function:

 void SolveConstraintProblem ( const ConstraintAccelProblemData< T > & problem_data, VectorX< T > * cf ) const

Solves the appropriate constraint problem at the acceleration level.

Parameters
 problem_data The data used to compute the constraint forces. cf The computed constraint forces, on return, in a packed storage format. The first nc elements of cf correspond to the magnitudes of the contact forces applied along the normals of the nc contact points. The next elements of cf correspond to the frictional forces along the r spanning directions at each non-sliding point of contact. The first r values (after the initial nc elements) correspond to the first non-sliding contact, the next r values correspond to the second non-sliding contact, etc. The next ℓ values of cf correspond to the forces applied to enforce generic unilateral constraints. The final b values of cf correspond to the forces applied to enforce generic bilateral constraints. This packed storage format can be turned into more useful representations through ComputeGeneralizedForceFromConstraintForces() and CalcContactForcesInContactFrames(). cf will be resized as necessary.
Precondition
Constraint data has been computed.
Exceptions
 a std::runtime_error if the constraint forces cannot be computed (due to, e.g., an "inconsistent" rigid contact configuration). a std::logic_error if cf is null.

Here is the call graph for this function:

 void SolveImpactProblem ( const ConstraintVelProblemData< T > & problem_data, VectorX< T > * cf ) const

Solves the impact problem described above.

Parameters
 problem_data The data used to compute the impulsive constraint forces. cf The computed impulsive forces, on return, in a packed storage format. The first nc elements of cf correspond to the magnitudes of the contact impulses applied along the normals of the nc contact points. The next elements of cf correspond to the frictional impulses along the r spanning directions at each point of contact. The first r values (after the initial nc elements) correspond to the first contact, the next r values correspond to the second contact, etc. The next ℓ values of cf correspond to the impulsive forces applied to enforce unilateral constraint functions. The final b values of cf correspond to the forces applied to enforce generic bilateral constraints. This packed storage format can be turned into more useful representations through ComputeGeneralizedForceFromConstraintForces() and CalcContactForcesInContactFrames(). cf will be resized as necessary.
Precondition
Constraint data has been computed.
Exceptions
 a std::runtime_error if the constraint forces cannot be computed (due to, e.g., the effects of roundoff error in attempting to solve a complementarity problem); in such cases, it is recommended to increase regularization and attempt again. a std::logic_error if cf is null.

Here is the call graph for this function:

Here is the caller graph for this function:

 void UpdateDiscretizedTimeLcp ( const ConstraintVelProblemData< T > & problem_data, double h, MlcpToLcpData * mlcp_to_lcp_data, VectorX< T > * a, MatrixX< T > * MM, VectorX< T > * qq )
static

Updates the time-discretization of the LCP initially computed in ConstructBaseDiscretizedTimeLcp() using the problem data and time step h.

Solving the resulting pure LCP yields non-impulsive constraint forces that can be obtained from PopulatePackedConstraintForcesFromLcpSolution().

Parameters
 problem_data the constraint problem data. [out] mlcp_to_lcp_data a pointer to a valid MlcpToLcpData object; the caller must ensure that this pointer remains valid through the constraint solution process. [out] a the vector corresponding to the MLCP vector a, on return. [out] MM a pointer to the updated LCP matrix on return. [out] qq a pointer to the updated LCP vector on return.
Precondition
mlcp_to_lcp_data, a, MM, and qq` are non-null on entry.