Drake

Solves constraint problems for constraint forces. More...
#include <drake/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  
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  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...  
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 impulses expressed in each contact frame for 2D contact problems from the "packed" solution returned by SolveImpactProblem().
cf  the output from SolveImpactProblem()  
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_impulses  a nonnull vector of a doublet of values, where the iᵗʰ element represents the impulsive force along each basis vector in the iᵗʰ contact frame. 
std::logic_error  if 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. 
contact_frames[i]
* contact_impulses[i]
.

static 
Computes the system generalized acceleration, given the external forces (stored in problem_data
) and the 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 impulse on the system from the constraint impulses given in packed storage.
problem_data  The data used to compute the constraint impulses.  
cf  The computed constraint impulses, in the packed storage format described in documentation for SolveImpactProblem.  
[out]  generalized_impulse  The 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 . 
std::logic_error  if 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.

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. 

delete 

delete 
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 appropriate impact problem at the velocity level.
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 ComputeGeneralizedImpulseFromConstraintImpulses() and CalcImpactForcesInContactFrames(). 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. 