Drake
ConstraintAccelProblemData< T > Struct Template Reference

Structure for holding constraint data for computing forces due to constraints and the resulting multibody accelerations. More...

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

Collaboration diagram for ConstraintAccelProblemData< T >:
[legend]

Public Member Functions

 ConstraintAccelProblemData (int gv_dim)
 Constructs acceleration problem data for a system with a gv_dim dimensional generalized velocity. More...
 

Public Attributes

bool use_complementarity_problem_solver {true}
 Flag for whether the complementarity problem solver should be used to solve this particular problem instance. More...
 
std::vector< intsliding_contacts
 The indices of the sliding contacts (those contacts at which there is non-zero relative velocity between bodies in the plane tangent to the point of contact), out of the set of all contact indices (0...nc-1). More...
 
std::vector< intnon_sliding_contacts
 The indices of the non-sliding contacts (those contacts at which there is zero relative velocity between bodies in the plane tangent to the point of contact), out of the set of all contact indices (0...nc-1). More...
 
std::vector< intr
 The number of spanning vectors in the contact tangents (used to linearize the friction cone) at the n non-sliding contact points. More...
 
VectorX< T > mu_sliding
 Coefficients of friction for the ns = nc - nns sliding contacts (where nns is the number of non-sliding contacts). More...
 
VectorX< T > mu_non_sliding
 Coefficients of friction for the nns = nc - ns non-sliding contacts (where ns is the number of sliding contacts). More...
 
VectorX< T > tau
 The ℝⁿᵛ vector tau, the generalized external force vector that comprises gravitational, centrifugal, Coriolis, actuator, etc. More...
 
std::function< MatrixX< T >const MatrixX< T > &)> solve_inertia
 A function for solving the equation MX = B for matrix X, given input matrix B, where M is the generalized inertia matrix for the rigid body system. More...
 
Data for bilateral constraints at the acceleration level

Problem data for bilateral constraints of functions of system acceleration, where the constraint can be formulated as:

0 = G(q)⋅v̇ + kᴳ(t,q,v)

which implies the constraint definition g(t,q,v,v̇) ≡ G(q)⋅v̇ + kᴳ(t,q,v). G is defined as the ℝⁿᵇˣⁿᵛ Jacobian matrix that transforms generalized velocities (v ∈ ℝⁿᵛ) into the time derivatives of b bilateral constraint functions. The class of constraint functions naturally includes holonomic constraints, which are constraints posable as g(t,q). Such holonomic constraints must be twice differentiated with respect to time to yield an acceleration-level formulation (i.e., g̈(t, q, v, v̇), for the aforementioned definition of g(t,q)). That differentiation yields g̈ = G⋅v̇ + dG/dt⋅v, which is consistent with the constraint class under the definition kᴳ(t,q,v) ≡ dG/dt⋅v. An example such (holonomic) constraint function is the transmission (gearing) constraint below:

0 = v̇ᵢ - rv̇ⱼ

which can be read as the acceleration at joint i (v̇ᵢ) must equal to r times the acceleration at joint j (v̇ⱼ); r is thus the gear ratio. In this example, the corresponding holonomic constraint function is g(q) ≡ qᵢ - rqⱼ, yielding ̈g(q, v, v̇) = v̇ᵢ - rv̇ⱼ.

std::function< VectorX< T >const VectorX< T > &)> G_mult
 An operator that performs the multiplication G⋅v. More...
 
std::function< VectorX< T >const VectorX< T > &)> G_transpose_mult
 An operator that performs the multiplication Gᵀ⋅f where f ∈ ℝⁿᵇ are the magnitudes of the constraint forces. More...
 
VectorX< T > kG
 This ℝⁿᵇ vector is the vector kᴳ(t,q,v) defined above. More...
 
Data for constraints on accelerations along the contact normal

Problem data for constraining the acceleration of two bodies projected along the contact surface normal, for n point contacts.

These data center around two Jacobian matrices, N and Q. N is the ℝⁿᶜˣⁿᵛ Jacobian matrix that transforms generalized velocities (v ∈ ℝⁿᵛ) into velocities projected along the contact normals at the nc point contacts. Q ∈ ℝⁿᶜˣⁿᵛ is the Jacobian matrix that transforms generalized velocities (nv is the dimension of generalized velocity) into velocities projected along the directions of sliding at the ns sliding contact points (rows of Q that correspond to non-sliding contacts should be zero). Finally, the Jacobian matrix N allows formulating the non-interpenetration constraint (a constraint imposed at the velocity level) as:

0 ≤ N(q)⋅v̇ + kᴺ(t,q,v)  ⊥  fᶜ ≥ 0

which means that the constraint g̈(q,v,v̇) ≡ N(q)⋅v̇ + kᴺ(t,q,v) is coupled to a force constraint (fᶜ ≥ 0) and a complementarity constraint fᶜ⋅(Nv̇ + kᴺ(t,q,v)) = 0, meaning that the constraint can apply no force if it is inactive (i.e., if g̈(q,v,v̇) is strictly greater than zero). Note that differentiating the original constraint ġ(t,q,v) ≡ Nv (i.e., the constraint posed at the velocity level) once with respect to time, such that all constraints are imposed at the acceleration level, yields:

g̈(t,q,v,v̇) = N(q) v̇ + dN/dt(q,v) v

Thus, the constraint at the acceleration level can be realized by setting kᴺ(t,q,v) = dN/dt(q,v)⋅v. If there is pre-existing constraint error (e.g., if N(q)⋅v < 0), the kᴺ term can be used to "stabilize" this error. For example, one could set kᴺ(t,q,v) = dN/dt(q,v)⋅v + α⋅N(q)⋅v, for α ≥ 0.

std::function< VectorX< T >const VectorX< T > &)> N_mult
 An operator that performs the multiplication N⋅v. More...
 
std::function< VectorX< T >const VectorX< T > &)> N_minus_muQ_transpose_mult
 An operator that performs the multiplication (Nᵀ - μQᵀ)⋅f, where μ is a diagonal matrix with nonzero entries corresponding to the coefficients of friction at the s sliding contact points, and (Nᵀ - μQᵀ) transforms forces (f ∈ ℝⁿᶜ) applied along the contact normals at the nc point contacts into generalized forces. More...
 
VectorX< T > kN
 This ℝⁿᶜ vector is the vector kᴺ(t,q,v) defined above. More...
 
Data for non-sliding contact friction constraints

Problem data for constraining the tangential acceleration of two bodies projected along the contact surface tangents, for nc point contacts.

These data center around the Jacobian matrix, F ∈ ℝⁿⁿʳˣⁿᵛ, that transforms generalized velocities (v ∈ ℝⁿᵛ) into velocities projected along the nr vectors that span the contact tangents at the nns non-sliding point contacts (these nns * nr vectors are denoted nnr, like "total number of nr", for brevity). For contact problems in two dimensions, nr will be one and nnr would equal nns. For friction pyramids at each contact in three dimensions, nr would be two and nnr would equal 2nns. While the definition of nnr above indicates that every one of the nns non-sliding contacts uses the same "nr", the code imposes no such requirement. Finally, the Jacobian matrix F allows formulating the non-sliding friction force constraints as:

0 ≤ F(q)⋅v̇ + kᶠ(t,q,v) + λe  ⊥  fᶜ ≥ 0

which means that the constraint g̈(t,q,v,v̇) ≡ F(q)⋅v̇ + kᶠ(t,q,v) is coupled to a force constraint (fᶜ ≥ 0) and a complementarity constraint fᶜ⋅(Fv̇ + kᴺ(t,q,v) + λe) = 0: the constraint can apply no force if it is inactive (i.e., if g̈(t,q,v,v̇) is strictly greater than zero). The presence of the λe term is taken directly from [Anitescu 1997], where e is a vector of ones and zeros and λ corresponds roughly to the tangential acceleration at the contacts. The interested reader should refer to [Anitescu 1997] for a more thorough explanation of this constraint; the full constraint equation is presented only to elucidate the purpose of the kᶠ term. Analogously to the case of kᴺ, kᶠ should be set to dF/dt(q,v)⋅v; also analogously, kᶠ can be used to perform constraint stabilization.

std::function< VectorX< T >const VectorX< T > &)> F_mult
 An operator that performs the multiplication F⋅v. More...
 
std::function< VectorX< T >const VectorX< T > &)> F_transpose_mult
 An operator that performs the multiplication Fᵀ⋅f, where f ∈ ℝⁿⁿˢʳ corresponds to frictional force magnitudes. More...
 
VectorX< T > kF
 This ℝⁿⁿˢʳ vector is the vector kᶠ(t,q,v) defined above. More...
 
Data for unilateral constraints at the acceleration level

Problem data for unilateral constraints of functions of system acceleration, where the constraint can be formulated as:

0 ≤ L(q)⋅v̇ + kᴸ(t,q,v)  ⊥  fᶜ ≥ 0

which means that the constraint g(q,v,v̇) ≡ L(q)⋅v̇ + kᴸ(t,q,v) is coupled to a force constraint (fᶜ ≥ 0) and a complementarity constraint fᶜ⋅(L⋅v̇ + kᴸ(t,q,v)) = 0, meaning that the constraint can apply no force if it is inactive (i.e., if g(q,v,v̇) is strictly greater than zero). L is defined as the ℝⁿᵘˣⁿᵛ Jacobian matrix that transforms generalized velocities (v ∈ ℝⁿᵛ) into the time derivatives of nu unilateral constraint functions. The class of constraint functions naturally includes holonomic constraints, which are constraints posable as g(t,q). Such holonomic constraints must be twice differentiated with respect to time to yield an acceleration-level formulation (i.e., g̈(q, v, v̇, t), for the aforementioned definition of g(t,q)). That differentiation yields g̈ = L⋅v̇ + dL/dt⋅v, which is consistent with the constraint class under the definition kᴸ(t,q,v) ≡ dL/dt⋅v. An example such holonomic constraint function is a joint acceleration limit:

0 ≤ -v̇ⱼ + r  ⊥  fᶜⱼ ≥ 0

which can be read as the acceleration at joint j (v̇ⱼ) must be no larger than r, the force must be applied to limit the acceleration at the joint, and the limiting force cannot be applied if the acceleration at the joint is not at the limit (i.e., v̇ⱼ < r). In this example, the corresponding holonomic constraint function is g(t,q) ≡ -qⱼ + rt², yielding ̈g(q, v, v̇) = -v̇ⱼ + r.

std::function< VectorX< T >const VectorX< T > &)> L_mult
 An operator that performs the multiplication L⋅v. More...
 
std::function< VectorX< T >const VectorX< T > &)> L_transpose_mult
 An operator that performs the multiplication Lᵀ⋅f where f ∈ ℝⁿᵘ are the magnitudes of the constraint forces. More...
 
VectorX< T > kL
 This ℝⁿᵘ vector is the vector kᴸ(t,q,v) defined above. More...
 

Detailed Description

template<class T>
struct drake::multibody::constraint::ConstraintAccelProblemData< T >

Structure for holding constraint data for computing forces due to constraints and the resulting multibody accelerations.

The Newton-Euler equations (essentially F = ma) coupled with constraints on the positional coordinates g(q) yields an Index-3 DAE (see [Hairer 1996]), and generally makes initial value problems hard to solve, computationally speaking; coupling the Newton-Euler equations with the second time derivative of such constraint equations (i.e., g̈(q,v,v̇)) yields a far more manageable Index-1 DAE, again with regard to computation. This structure stores problem data for computing dynamics under such constraints and others (nonholonomic constraints, Coulomb friction constraints, etc.)

Definition of variables specific to this class

(See Variable definitions) for the more general set of definitions).

  • ns ∈ ℕ The number of contacts at which sliding is occurring. Note that nc = ns + nns, where nc is the number of points of contact.
  • nns ∈ ℕ The number of contacts at which sliding is not occurring. Note that nc = ns + nns.

Constructor & Destructor Documentation

ConstraintAccelProblemData ( int  gv_dim)
inlineexplicit

Constructs acceleration problem data for a system with a gv_dim dimensional generalized velocity.

Member Data Documentation

std::function<VectorX<T>const VectorX<T>&)> F_mult

An operator that performs the multiplication F⋅v.

The default operator returns an empty vector.

std::function<VectorX<T>const VectorX<T>&)> F_transpose_mult

An operator that performs the multiplication Fᵀ⋅f, where f ∈ ℝⁿⁿˢʳ corresponds to frictional force magnitudes.

The default operator returns a zero vector of dimension equal to that of the generalized forces.

std::function<VectorX<T>const VectorX<T>&)> G_mult

An operator that performs the multiplication G⋅v.

The default operator returns an empty vector.

std::function<VectorX<T>const VectorX<T>&)> G_transpose_mult

An operator that performs the multiplication Gᵀ⋅f where f ∈ ℝⁿᵇ are the magnitudes of the constraint forces.

The default operator returns a zero vector of dimension equal to that of the generalized forces.

VectorX<T> kF

This ℝⁿⁿˢʳ vector is the vector kᶠ(t,q,v) defined above.

VectorX<T> kG

This ℝⁿᵇ vector is the vector kᴳ(t,q,v) defined above.

VectorX<T> kL

This ℝⁿᵘ vector is the vector kᴸ(t,q,v) defined above.

VectorX<T> kN

This ℝⁿᶜ vector is the vector kᴺ(t,q,v) defined above.

std::function<VectorX<T>const VectorX<T>&)> L_mult

An operator that performs the multiplication L⋅v.

The default operator returns an empty vector.

std::function<VectorX<T>const VectorX<T>&)> L_transpose_mult

An operator that performs the multiplication Lᵀ⋅f where f ∈ ℝⁿᵘ are the magnitudes of the constraint forces.

The default operator returns a zero vector of dimension equal to that of the generalized forces.

VectorX<T> mu_non_sliding

Coefficients of friction for the nns = nc - ns non-sliding contacts (where ns is the number of sliding contacts).

The size of this vector should be equal to non_sliding_contacts.size().

VectorX<T> mu_sliding

Coefficients of friction for the ns = nc - nns sliding contacts (where nns is the number of non-sliding contacts).

The size of this vector should be equal to sliding_contacts.size().

std::function<VectorX<T>const VectorX<T>&)> N_minus_muQ_transpose_mult

An operator that performs the multiplication (Nᵀ - μQᵀ)⋅f, where μ is a diagonal matrix with nonzero entries corresponding to the coefficients of friction at the s sliding contact points, and (Nᵀ - μQᵀ) transforms forces (f ∈ ℝⁿᶜ) applied along the contact normals at the nc point contacts into generalized forces.

The default operator returns a zero vector of dimension equal to that of the generalized forces.

std::function<VectorX<T>const VectorX<T>&)> N_mult

An operator that performs the multiplication N⋅v.

The default operator returns an empty vector.

std::vector<int> non_sliding_contacts

The indices of the non-sliding contacts (those contacts at which there is zero relative velocity between bodies in the plane tangent to the point of contact), out of the set of all contact indices (0...nc-1).

This vector must be in sorted order.

The number of spanning vectors in the contact tangents (used to linearize the friction cone) at the n non-sliding contact points.

For contact problems in two dimensions, each element of r will be one. For contact problems in three dimensions, a friction pyramid (for example), for a contact point i will have rᵢ = 2. [Anitescu 1997] define k such vectors and require that, for each vector w in the spanning set, -w also exists in the spanning set. The RigidContactAccelProblemData structure expects that the contact solving mechanism negates the spanning vectors so r = k/2 spanning vectors will correspond to a k-edge polygon friction cone approximation.

std::vector<int> sliding_contacts

The indices of the sliding contacts (those contacts at which there is non-zero relative velocity between bodies in the plane tangent to the point of contact), out of the set of all contact indices (0...nc-1).

This vector must be in sorted order.

std::function<MatrixX<T>const MatrixX<T>&)> solve_inertia

A function for solving the equation MX = B for matrix X, given input matrix B, where M is the generalized inertia matrix for the rigid body system.

VectorX<T> tau

The ℝⁿᵛ vector tau, the generalized external force vector that comprises gravitational, centrifugal, Coriolis, actuator, etc.

forces applied to the rigid body system at q. m is the dimension of the generalized force, which is also equal to the dimension of the generalized velocity.

bool use_complementarity_problem_solver {true}

Flag for whether the complementarity problem solver should be used to solve this particular problem instance.

If every constraint in the problem data is active, using the linear system solver (use_complementarity_problem_solver=false) will yield a solution much more quickly. If it is unknown whether every constraint is active, the complementarity problem solver should be used; otherwise, the inequality constraints embedded in the problem data may not be satisfied. The safe (and slower) value of true is the default.


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