Drake
ConstraintSolver< T > Class Template Reference

Solves constraint problems for constraint forces. More...

#include <multibody/constraint/constraint_solver.h>

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...
 
void SolveImpactProblem (const ConstraintVelProblemData< T > &problem_data, VectorX< T > *cf) const
 Solves the appropriate impact problem at the velocity 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 ComputeGeneralizedImpulseFromConstraintImpulses (const ConstraintVelProblemData< T > &problem_data, const VectorX< T > &cf, VectorX< T > *generalized_impulse)
 Computes the generalized impulse on the system from the constraint impulses 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, given the external forces (stored in problem_data) and the 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 CalcImpactForcesInContactFrames (const VectorX< T > &cf, const ConstraintVelProblemData< T > &problem_data, const std::vector< Matrix2< T >> &contact_frames, std::vector< Vector2< T >> *contact_impulses)
 Gets the contact impulses expressed in each contact frame for 2D contact problems from the "packed" solution returned by SolveImpactProblem(). 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
TThe 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
cfthe output from SolveConstraintProblem()
problem_datathe problem data input to SolveConstraintProblem()
contact_framesthe 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_forcesa 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_errorif 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:

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

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

Parameters
cfthe output from SolveImpactProblem()
problem_datathe problem data input to SolveImpactProblem()
contact_framesthe 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_impulsesa non-null vector of a doublet of values, where the iᵗʰ element represents the impulsive force along each basis vector in the iᵗʰ contact frame.
Exceptions
std::logic_errorif contact_impulses is null, if contact_impulses 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 impulse at the iᵗʰ contact point expressed in the world frame is contact_frames[i] * contact_impulses[i].

Here is the call graph for this function:

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

Computes the system generalized acceleration, given the external forces (stored in problem_data) and the constraint forces.

Parameters
cfThe computed constraint forces, in the packed storage format described in documentation for SolveConstraintProblem.
Exceptions
std::logic_errorif 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_dataThe data used to compute the contact forces.
cfThe computed constraint forces, in the packed storage format described in documentation for SolveConstraintProblem.
[out]generalized_forceThe 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_errorif 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 ComputeGeneralizedImpulseFromConstraintImpulses ( const ConstraintVelProblemData< T > &  problem_data,
const VectorX< T > &  cf,
VectorX< T > *  generalized_impulse 
)
static

Computes the generalized impulse on the system from the constraint impulses given in packed storage.

Parameters
problem_dataThe data used to compute the constraint impulses.
cfThe computed constraint impulses, in the packed storage format described in documentation for SolveImpactProblem.
[out]generalized_impulseThe generalized impulse acting on the system from the total constraint wrench is stored here, on return. This method will resize generalized_impulse as necessary. The indices of generalized_impulse will exactly match the indices of problem_data.v.
Exceptions
std::logic_errorif generalized_impulse is null or cf vector is incorrectly sized.

Get the normal and tangential contact impulses.

Get the limit forces.

Compute the generalized impules.

Here is the caller graph for this function:

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
cfThe computed constraint impulses, in the packed storage format described in documentation for SolveImpactProblem.
Exceptions
std::logic_errorif generalized_delta_v is null or cf vector is incorrectly sized.

Here is the call graph for this function:

ConstraintSolver& operator= ( ConstraintSolver< T > &&  )
delete
ConstraintSolver& operator= ( const ConstraintSolver< T > &  )
delete
void SolveConstraintProblem ( const ConstraintAccelProblemData< T > &  problem_data,
VectorX< T > *  cf 
) const

Solves the appropriate constraint problem at the acceleration level.

Parameters
problem_dataThe data used to compute the constraint forces.
cfThe 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
astd::runtime_error if the constraint forces cannot be computed (due to, e.g., an "inconsistent" rigid contact configuration).
astd::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 appropriate impact problem at the velocity level.

Parameters
problem_dataThe data used to compute the impulsive constraint forces.
cfThe 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 ComputeGeneralizedImpulseFromConstraintImpulses() and CalcImpactForcesInContactFrames(). cf will be resized as necessary.
Precondition
Constraint data has been computed.
Exceptions
astd::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.
astd::logic_error if cf is null.

Here is the call graph for this function:


The documentation for this class was generated from the following file: