Drake

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  
ConstraintSolver &  operator= (const ConstraintSolver &)=delete 
ConstraintSolver (ConstraintSolver &&)=delete  
ConstraintSolver &  operator= (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 firstorder 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...  
Velocitylevel 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 (e) u = A⁻¹ (a + X₁y) allowing the mixed LCP to be converted to a "pure" LCP (f) qq = x₄  X₂A⁻¹a (g) MM = X₃  X₂A⁻¹X₁ This pure LCP can then be solved for (h) MMv + qq ≥ 0 (i) y ≥ 0 (j) yᵀ(MMv + qq) = 0 and this value for An MLCPbased 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 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 activesettype mathematical programming approaches: A ≡  M Ĝᵀ a ≡ Mv⁻  X₁ ≡ Nᵀ Dᵀ 0 Lᵀ   Ĝ 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 rankdeficient 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 y ≡  fN   fD   λ   fL  The key variables for using the MLCPbased formulations are the matrix Another use of the MLCP formulation (discretized multibody dynamics with contact and friction):Without reconstructing the entire MLCP, we now show a very similar formulation to solve the problem of discretizing the multibody dynamics equations with contact and friction. This particular formulation provides several nice features: 1) the formulation is semiimplicit 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 nonconstraint forces, which are nonimpulsive); and 3) can be made almost symplectic by choosing a representation and computational coordinate frame that minimize velocitydependent forces (thereby explaining the extreme stability of software like ODE and Bullet that computes dynamics in body frames (minimizing the magnitudes of velocitydependent 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 The procedure one uses to formulate and solve this discretization problem is:
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 nonimpulsive), 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 timediscretization of the system using the problem data, resulting in the MM and qq described in velocitylevelMLCPs; 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 timediscretization 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...  
Solves constraint problems for constraint forces.
Specifically, given problem data corresponding to a rigid or multibody 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 semidefinite 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 multibody dynamics simulation libraries.
T  The vector element type, which must be a valid Eigen scalar. 
Instantiated templates for the following scalar types T
are provided:

default 

delete 

delete 

static 
Gets the contact forces expressed in each contact frame for 2D contact problems from the "packed" solution returned by SolveConstraintProblem().
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 nonsliding 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 nonnull vector of a doublet of values, where the iᵗʰ element represents the force along each basis vector in the iᵗʰ contact frame. 
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 nonsliding 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. 
contact_frames[i]
* contact_forces[i]
.

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 nonimpulsive, the contact forces will be nonimpulsive (with units of N).
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 nonnull vector of a doublet of values, where the iᵗʰ element represents the force along each basis vector in the iᵗʰ contact frame. 
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. 
contact_frames[i]
* contact_forces[i]
.

inlinestatic 
Computes the system generalized acceleration due to both external forces and constraint forces.
problem_data  The accelerationlevel 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. 
std::logic_error  if generalized_acceleration is null or cf vector is incorrectly sized. 

static 
Computes a firstorder approximation of generalized acceleration due only to constraint forces.
problem_data  The velocitylevel 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. 
problem_data.solve_inertia()
in order to compute v(t+dt)
, so the computational demands may be significant. std::logic_error  if generalized_acceleration is null or cf vector is incorrectly sized. 
dt
is positive.

static 
Computes the system generalized acceleration due only to constraint forces.
cf  The computed constraint forces, in the packed storage format described in documentation for SolveConstraintProblem. 
std::logic_error  if generalized_acceleration is null or cf vector is incorrectly sized. 

static 
Computes the system generalized acceleration due only to constraint forces.
cf  The computed constraint forces, in the packed storage format described in documentation for SolveConstraintProblem. 
std::logic_error  if generalized_acceleration is null or cf vector is incorrectly sized. 

static 
Computes the generalized force on the system from the constraint forces given in packed storage.
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 . 
std::logic_error  if generalized_force is null or cf vector is incorrectly sized. 
Get the normal and nonsliding contact forces.
Get the limit forces.
Compute the generalized force.

static 
Computes the generalized force on the system from the constraint forces given in packed storage.
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 . 
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.

static 
Computes the change to the system generalized velocity from constraint impulses.
cf  The computed constraint impulses, in the packed storage format described in documentation for SolveImpactProblem. 
std::logic_error  if generalized_delta_v is null or cf vector is incorrectly sized. 

static 
Computes the base timediscretization of the system using the problem data, resulting in the MM
and qq
described in velocitylevelMLCPs; 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 nonimpulsive problem formulation. In that case, the multibody dynamics equations are discretized, as described in UpdateDiscretizedTimeLcp().
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]  a pointer to a vector that will contain the parts of the LCP vector not dependent upon the time step on return. 
mlcp_to_lcp_data
, MM
, and qq
are nonnull on entry.

delete 

delete 

static 
Populates the packed constraint force vector from the solution to the linear complementarity problem (LCP) constructed using ConstructBaseDiscretizedTimeLcp() and UpdateDiscretizedTimeLcp().
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(). 
void SolveConstraintProblem  (  const ConstraintAccelProblemData< T > &  problem_data, 
VectorX< T > *  cf  
)  const 
Solves the appropriate constraint problem at the acceleration level.
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 nonsliding point of contact. The first r values (after the initial nc elements) correspond to the first nonsliding contact, the next r values correspond to the second nonsliding 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. 
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. 
void SolveImpactProblem  (  const ConstraintVelProblemData< T > &  problem_data, 
VectorX< T > *  cf  
)  const 
Solves the impact problem described above.
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. 
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. 

static 
Updates the timediscretization of the LCP initially computed in ConstructBaseDiscretizedTimeLcp() using the problem data and time step h
.
Solving the resulting pure LCP yields nonimpulsive constraint forces that can be obtained from PopulatePackedConstraintForcesFromLcpSolution().
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]  a pointer to the updated LCP vector on return. 
mlcp_to_lcp_data
, a
, MM
, and qq
are nonnull on entry.