Drake

ImplicitStribeckSolver solves the equations below for mechanical systems with contact using a modified Stribeck model of friction:
q̇ = N(q) v (1) M(q) v̇ = τ + Jₙᵀ(q) fₙ(q, v) + Jₜᵀ(q) fₜ(q, v)
where v ∈ ℝⁿᵛ
is the vector of generalized velocities, M(q) ∈ ℝⁿᵛˣⁿᵛ
is the mass matrix, Jₙ(q) ∈ ℝⁿᶜˣⁿᵛ
is the Jacobian of normal separation velocities, Jₜ(q) ∈ ℝ²ⁿᶜˣⁿᵛ
is the Jacobian of tangent velocities, fₙ ∈ ℝⁿᶜ
is the vector of normal contact forces, fₜ ∈ ℝ²ⁿᶜ
is the vector of tangent friction forces and τ ∈ ℝⁿᵛ is a vector of generalized forces containing all other applied forces (e.g., Coriolis, gyroscopic terms, actuator forces, etc.) but contact forces. This solver assumes a compliant law for the normal forces fₙ(q, v)
and therefore the functional dependence of fₙ(q, v)
with q and v is stated explicitly.
Since ImplicitStribeckSolver uses a modified Stribeck model for friction, we explicitly emphasize the functional dependence of fₜ(q, v)
with the generalized velocities. The functional dependence of fₜ(q, v)
with the generalized positions stems from its direct dependence with the normal forces fₙ(q, v)
.
ImplicitStribeckSolver implements two different schemes. A "oneway coupling scheme" which solves for the friction forces given the normal forces are known. That is, normal forces affect the computation of the friction forces however, the normal forces are kept constant during the solution procedure.
A "twoway coupling scheme" treats both the normal and friction forces implicitly in the generalized velocities resulting in a numerical strategy in which normal forces affect friction forces and, conversely, friction forces couple back to the computation of the normal forces.
The twoway coupled scheme provides a more stable and therefore robust solution to the problem stated in Eq. (1) with just a small increase of the computational cost compared to the oneway coupled scheme. The oneway coupled scheme is however very useful for testing and analysis of the solver.
Equation (1) is discretized in time using a variation of the first order semiimplicit Euler scheme from step s to step s+1 with time step δt
as:
qˢ⁺¹ = qˢ + δt N(qˢ) vˢ⁺¹ (2) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ + δt [τˢ + Jₙᵀ(qˢ) fₙ(qˢ,vˢ) + Jₜᵀ(qˢ) fₜ(qˢ,vˢ⁺¹)]
(We are using s for step counter rather than n to avoid Unicodeinduced confusion with the "normal direction" subscript n.)
Please see details in the Derivation of the oneway coupling scheme section. The equation for the generalized velocities in Eq. (2) is rewritten as:
(3) M vˢ⁺¹ = p* + δt [Jₙᵀ fₙ + Jₜᵀ fₜ(vˢ⁺¹)]
where p* = M vˢ + δt τˢ
is the generalized momentum that the system would have in the absence of contact forces and, for simplicity, we have only kept the functional dependencies in generalized velocities. Notice that ImplicitStribeckSolver uses a precomputed value of the normal forces. These normal forces could be available for instance if using a compliant contact approach, for which normal forces are a function of the state.
Equation (1) is discretized in time using a variation on the semiimplicit Euler scheme with time step δt
as:
qˢ⁺¹ = qˢ + δt N(qˢ) vˢ⁺¹ (4) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ + δt [τˢ + Jₙᵀ(qˢ) fₙˢ⁺¹ + Jₜᵀ(qˢ) fₜ(fₙˢ⁺¹,vₜˢ⁺¹)]
This implicit scheme variation evaluates Jacobian matrices Jₙ and Jₜ as well as the kinematic mapping N(q) at the previous time step. In Eq. (4) we have fₙˢ⁺¹ = fₙ(xˢ⁺¹, vₙˢ⁺¹) with xˢ⁺¹ = x(qˢ⁺¹), the signed penetration distance (defined positive when bodies overlap) between contact point pairs and the separation velocities vₙˢ⁺¹ = Jₙ(qˢ) vˢ⁺¹ (= −ẋˢ⁺¹). Also the functional dependence of fₜ with fₙ and vₜ is highlighted in Eq. (4). More precisely, the twoway coupling scheme uses a normal force law for each contact pair of the form:
(5) fₙ(x, vₙ) = k(vₙ)₊ x₊ (6) k(vₙ) = k (1 − d vₙ)₊
where x₊
is the positive part of x (x₊ = x if x ≥ 0 and x₊ = 0 otherwise) and k
and d are the stiffness and dissipation coefficients for a given contact point, respectively.
The twoway coupling scheme uses a first order approximation to the signed distance functions vector:
(7) xˢ⁺¹ ≈ xˢ − δt vₙˢ⁺¹ = xˢ − δt Jₙ(qˢ) vˢ⁺¹
where the minus sign is needed given that ẋ = dx/dt = −vₙ. This approximation is used in Eq. (5) to obtain a numerical scheme that implicitly couples normal forces through their functional dependence on the signed penetration distance. Notice that, according to Eq. (5), normal forces at each contact point are decoupled from each other. However their values are coupled given the choice of a common variable, the generalized velocity v.
Equation (7) is used into Eq. (5) to obtain an expression of the normal force in terms of the generalized velocity vˢ⁺¹ at the next time step:
(8) fₙ(xˢ⁺¹, vₙˢ⁺¹) = k (1 − d vₙˢ⁺¹)₊ xˢ⁺¹₊ = k (1 − d Jₙ(qˢ) vˢ⁺¹)₊ (xˢ − δt Jₙ(qˢ) vˢ⁺¹)₊ = fₙ(vˢ⁺¹)
Similarly, the friction forces fₜ can be written in terms of the next time step generalized velocities using vₜˢ⁺¹ = Jₜ(qˢ) vˢ⁺¹ and using Eq. (8) to substitute an expression for the normal force in terms of vˢ⁺¹. This allows to rewrite the tangential forces as a function of the generalized velocities as:
(9) fₜ(fₙˢ⁺¹, vₜˢ⁺¹) = fₜ(fₙ(x(vˢ⁺¹), vₙ(vˢ⁺¹)), vₜ((vˢ⁺¹))) = fₜ(vˢ⁺¹)
where we write x(vˢ⁺¹) = xˢ − δt Jₙ(qˢ) vˢ⁺¹. Finally, Eqs. (8) and (9) are used into Eq. (4) to obtain an expression in vˢ⁺¹:
(10) M(qˢ) vˢ⁺¹ = p* + δt [Jₙᵀ(qˢ) fₙ(vˢ⁺¹) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)]
with p* = p* = M vˢ + δt τˢ
the generalized momentum that the system would have in the absence of contact forces.
ImplicitStribeckSolver uses a NewtonRaphson strategy to solve Eq. (10) in the generalized velocities, limiting the iteration update with the scheme described in iteration_limiter.
ImplicitStribeckSolver solves for the generalized velocity at the next time step vˢ⁺¹
with either a oneway or twoway coupled scheme as described in the previous sections. The solver uses a NewtonRaphson iteration to compute an update Δvᵏ
at the kth NewtonRaphson iteration. Once Δvᵏ
is computed, the solver limits the change in the tangential velocities Δvₜᵏ = Jₜᵀ Δvᵏ
using the approach described in [Uchida et al., 2015]. This approach limits the maximum angle change θ between two successive iterations in the tangential velocity.
Uchida, T.K., Sherman, M.A. and Delp, S.L., 2015. Making a meaningful impact: modelling simultaneous frictional collisions in spatial multibody systems. Proc. R. Soc. A, 471(2177), p.20140859.
In this section we provide a detailed derivation of the first order time stepping approach in Eq. (2). We start from the continuous Eq. (1):
(1) M(q) v̇ = τ + Jₙᵀ(q) fₙ(q,v) + Jₜᵀ(q) fₜ(q,v)
we can discretize Eq. (1) in time using a first order semiimplicit Euler scheme in velocities:
(11) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ + δt [τˢ⁺¹ + Jₙᵀ(qˢ) fₙ(qˢ,vˢ⁺¹) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)] + O₁(δt²)
where the equality holds strictly since we included the leading terms in O(δt²)
. We use τˢ⁺¹ = τ(tˢ, qˢ, vˢ⁺¹)
for brevity in Eq. (11). When moving from the continuous Eq. (1) to the discrete version Eq. (11), we lost the nice property that our compliant normal forces are decoupled from the friction forces (both depend on the same unknown vˢ⁺¹ in Eq (11)). The reason is that Eq. (11) includes an integration over a small interval of size δt. To solve the discrete system in Eq. (11), we'd like to decouple the normal forces from the tangential forces again, which will require a new (though still valid) approximation. To do so we will expand in Taylor series the term fₙ(qˢ, vˢ⁺¹)
:
(12) fₙ(qˢ, vˢ⁺¹) = fₙ(qˢ,vˢ) + ∇ᵥfₙ(qˢ,vˢ) (vˢ⁺¹ − vˢ) + O₂(‖vˢ⁺¹ − vˢ‖²)
The difference between vˢ
and vˢ⁺¹
can be written as:
(13) vˢ⁺¹ − vˢ = δtv̇ˢ + δtO₃(δt²) = O₄(δt)
Substituting vˢ⁺¹ − vˢ
from Eq. (13) into Eq. (12) we arrive to:
(14) fₙ(qˢ, vˢ⁺¹) = fₙ(qˢ,vˢ) + ∇ᵥfₙ(qˢ,vˢ) O₄(δt) + O₅(δt²) = fₙ(qˢ,vˢ) + O₆(δt)
where O₅(δt²) = O₂(‖vˢ⁺¹ − vˢ‖²) = O₂(‖O₄(δt)‖²)
. A similar argument for τˢ⁺¹ shows it also differs in O(δt) from τˢ = τ(tˢ, qˢ, vˢ). We can now use Eq. (14) into Eq. (11) to arrive to:
(15) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ + δt [τˢ + Jₙᵀ(qˢ) (fₙ(qˢ,vˢ) + O₆(δt)] + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)) + O₁(δt²)
which we can rewrite as:
(16) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ + δt [τˢ + Jₙᵀ(qˢ) fₙ(qˢ, vˢ) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)] + O₇(δt²)
with O₇(δt²) = δt Jₙᵀ(qˢ) O₆(δt) + O₁(δt²)
. That is, Eq. (16) introduces the same order of approximation as in the semiimplicit method in Eq. (11). Up to this point we have made no approximations but we instead propagated the O(⋅)
leading terms. Therefore the equalities in the equations above are exact. To obtain an approximate time stepping scheme, we drop O₇(δt²)
(we neglect it) in Eq. (16) to arrive to a first order scheme:
(17) M(qˢ) vˢ⁺¹ = M(qˢ) vˢ + δt [τˢ + Jₙᵀ(qˢ) fₙ(qˢ,vˢ) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)]
Therefore, with the scheme in Eq. (17) we are able to decouple the computation of (compliant) normal forces from that of friction forces. A very important feature of this scheme however, is the explicit nature (in the velocities v) of the term associated with the normal forces (usually including dissipation in the normal direction), which will become unstable for a sufficiently large time step. However, for most applications in practice, the stability of the scheme is mostly determined by the explicit update of normal forces with positions, that is, Eq. (17) is explicit in positions through the normal forces fₙ(qˢ, vˢ)
. For many common applications, the explicit dependence of τˢ(tˢ, qˢ, vˢ)
on the previous time step velocities vˢ
determines the overall stability of the scheme, since this term can include velocity dependent contributions such as control forces and dampers. Notice that Eq. (12) introduces an expansion of fₙ
with an order of approximation consistent with the first order scheme as needed. Therefore, it propagates into a O(δt²)
term exactly as needed in Eq. (16).
T  Must be one of drake's default scalar types. 
#include <drake/multibody/plant/implicit_stribeck_solver.h>
Public Member Functions  
ImplicitStribeckSolver (int nv)  
Instantiates a solver for a problem with nv generalized velocities. More...  
void  SetOneWayCoupledProblemData (EigenPtr< const MatrixX< T >> M, EigenPtr< const MatrixX< T >> Jn, EigenPtr< const MatrixX< T >> Jt, EigenPtr< const VectorX< T >> p_star, EigenPtr< const VectorX< T >> fn, EigenPtr< const VectorX< T >> mu) 
Sets data for the problem to be solved as outlined by Eq. More...  
void  SetTwoWayCoupledProblemData (EigenPtr< const MatrixX< T >> M, EigenPtr< const MatrixX< T >> Jn, EigenPtr< const MatrixX< T >> Jt, EigenPtr< const VectorX< T >> p_star, EigenPtr< const VectorX< T >> x0, EigenPtr< const VectorX< T >> stiffness, EigenPtr< const VectorX< T >> dissipation, EigenPtr< const VectorX< T >> mu) 
Sets the problem data to solve the problem outlined in Eq. More...  
ImplicitStribeckSolverResult  SolveWithGuess (double dt, const VectorX< T > &v_guess) const 
Given an initial guess v_guess , this method uses a NewtonRaphson iteration to find a solution for the generalized velocities satisfying either Eq. More...  
const ImplicitStribeckSolverIterationStats &  get_iteration_statistics () const 
Returns statistics recorded during the last call to SolveWithGuess(). More...  
const ImplicitStribeckSolverParameters &  get_solver_parameters () const 
Returns the current set of parameters controlling the iteration process. More...  
void  set_solver_parameters (const ImplicitStribeckSolverParameters ¶meters) 
Sets the parameters to be used by the solver. More...  
Does not allow copy, move, or assignment  
ImplicitStribeckSolver (const ImplicitStribeckSolver &)=delete  
ImplicitStribeckSolver &  operator= (const ImplicitStribeckSolver &)=delete 
ImplicitStribeckSolver (ImplicitStribeckSolver &&)=delete  
ImplicitStribeckSolver &  operator= (ImplicitStribeckSolver &&)=delete 
Retrieving the solution  
This methods allow to retrieve the solution stored in the solver after the last call to SolveWithGuess().  
const VectorX< T > &  get_generalized_friction_forces () const 
Returns a constant reference to the most recent vector of generalized friction forces. More...  
Eigen::VectorBlock< const VectorX< T > >  get_normal_velocities () const 
Returns a constant reference to the most recent solution vector for normal separation velocities. More...  
const VectorX< T > &  get_generalized_contact_forces () const 
Returns a constant reference to the most recent vector of generalized contact forces, including both friction and normal forces. More...  
Eigen::VectorBlock< const VectorX< T > >  get_tangential_velocities () const 
Returns a constant reference to the most recent vector of tangential forces. More...  
const VectorX< T > &  get_generalized_velocities () const 
Returns a constant reference to the most recent vector of generalized velocities. More...  
Eigen::VectorBlock< const VectorX< T > >  get_normal_forces () const 
Returns a constant reference to the most recent vector of (repulsive) forces in the normal direction. More...  
Eigen::VectorBlock< const VectorX< T > >  get_friction_forces () const 
Returns a constant reference to the most recent vector of friction forces. More...  
Friends  
class  ImplicitStribeckSolverTester 

delete 

delete 

explicit 
Instantiates a solver for a problem with nv
generalized velocities.
std::exception  if nv is nonpositive. 
Eigen::VectorBlock<const VectorX<T> > get_friction_forces  (  )  const 
Returns a constant reference to the most recent vector of friction forces.
These friction forces are defined in accordance to the tangential velocities Jacobian Jₜ as documented in this class's documentation.
const VectorX<T>& get_generalized_contact_forces  (  )  const 
Returns a constant reference to the most recent vector of generalized contact forces, including both friction and normal forces.
const VectorX<T>& get_generalized_friction_forces  (  )  const 
Returns a constant reference to the most recent vector of generalized friction forces.
const VectorX<T>& get_generalized_velocities  (  )  const 
Returns a constant reference to the most recent vector of generalized velocities.
const ImplicitStribeckSolverIterationStats& get_iteration_statistics  (  )  const 
Returns statistics recorded during the last call to SolveWithGuess().
See IterationStats for details.
Eigen::VectorBlock<const VectorX<T> > get_normal_forces  (  )  const 
Returns a constant reference to the most recent vector of (repulsive) forces in the normal direction.
That is, the normal force is positive when the bodies push each other apart. Otherwise the normal force is zero, since contact forces can only be repulsive.
Eigen::VectorBlock<const VectorX<T> > get_normal_velocities  (  )  const 
Returns a constant reference to the most recent solution vector for normal separation velocities.
This method returns an Eigen::VectorBlock
referencing a vector of size nc
.
const ImplicitStribeckSolverParameters& get_solver_parameters  (  )  const 
Returns the current set of parameters controlling the iteration process.
See Parameters for details.
Eigen::VectorBlock<const VectorX<T> > get_tangential_velocities  (  )  const 
Returns a constant reference to the most recent vector of tangential forces.
This method returns an Eigen::VectorBlock
referencing a vector of size nc
.

delete 

delete 
void set_solver_parameters  (  const ImplicitStribeckSolverParameters &  parameters  ) 
Sets the parameters to be used by the solver.
See Parameters for details.
void SetOneWayCoupledProblemData  (  EigenPtr< const MatrixX< T >>  M, 
EigenPtr< const MatrixX< T >>  Jn,  
EigenPtr< const MatrixX< T >>  Jt,  
EigenPtr< const VectorX< T >>  p_star,  
EigenPtr< const VectorX< T >>  fn,  
EigenPtr< const VectorX< T >>  mu  
) 
Sets data for the problem to be solved as outlined by Eq.
(3) in this class's documentation:
(3) M v = p* + δt Jₙᵀ fₙ + δt Jₜᵀ fₜ(v)
Refer to this class's documentation for further details on the structure of the problem and the solution strategy. In the documented parameters below, nv
is the number of generalized velocities and nc
is the number of contact points.
[in]  M  The mass matrix of the system, of size nv x nv . 
[in]  Jn  The normal separation velocities Jacobian, of size nc x nv . 
[in]  Jt  The tangential velocities Jacobian, of size 2nc x nv . 
[in]  p_star  The generalized momentum the system would have at s + 1 if contact forces were zero. 
[in]  fn  A vector of size nc containing the normal force at each contact point. 
[in]  mu  A vector of size nc containing the friction coefficient at each contact point. The solver makes no distinction between static and dynamic coefficients of friction or, similarly, the solver assumes the static and dynamic coefficients of friction are the same. 
std::exception  if any of the data pointers are nullptr. 
std::exception  if the problem data sizes are not consistent as described above. 
std::exception  if SetTwoWayCoupledProblemData() was ever called on this solver. 
void SetTwoWayCoupledProblemData  (  EigenPtr< const MatrixX< T >>  M, 
EigenPtr< const MatrixX< T >>  Jn,  
EigenPtr< const MatrixX< T >>  Jt,  
EigenPtr< const VectorX< T >>  p_star,  
EigenPtr< const VectorX< T >>  x0,  
EigenPtr< const VectorX< T >>  stiffness,  
EigenPtr< const VectorX< T >>  dissipation,  
EigenPtr< const VectorX< T >>  mu  
) 
Sets the problem data to solve the problem outlined in Eq.
(10) in this class's documentation using a twoway coupled approach:
(10) M(qˢ) vˢ⁺¹ = p* + δt [Jₙᵀ(qˢ) fₙ(vˢ⁺¹) + Jₜᵀ(qˢ) fₜ(vˢ⁺¹)]
Refer to this class's documentation for further details on the structure of the problem and the solution strategy. In the documented parameters below, nv
is the number of generalized velocities and nc
is the number of contact points.
[in]  M  The mass matrix of the system, of size nv x nv . 
[in]  Jn  The normal separation velocities Jacobian, of size nc x nv . 
[in]  Jt  The tangential velocities Jacobian, of size 2nc x nv . 
[in]  p_star  The generalized momentum the system would have at n + 1 if contact forces were zero. 
[in]  x0  The signed penetration distance at the previous time step. It is defined positive when bodies overlap. 
[in]  stiffness  A vector of size nc storing at each ith entry the stiffness coefficient for the ith contact pair. 
[in]  dissipation  A vector of size nc storing at each ith entry the dissipation coefficient for the ith contact pair. 
[in]  mu  A vector of size nc containing the friction coefficient at each contact point. The solver makes no distinction between static and dynamic coefficients of friction or, similarly, the solver assumes the static and dynamic coefficients of friction are the same. 
std::exception  if any of the data pointers are nullptr. 
std::exception  if the problem data sizes are not consistent as described above. 
std::exception  if SetOneWayCoupledProblemData() was ever called on this solver. 
ImplicitStribeckSolverResult SolveWithGuess  (  double  dt, 
const VectorX< T > &  v_guess  
)  const 
Given an initial guess v_guess
, this method uses a NewtonRaphson iteration to find a solution for the generalized velocities satisfying either Eq.
(3) when oneway coupling is used or Eq. (10) when twoway coupling is used. See this class's documentation for further details. To retrieve the solution, please refer to retrieving_the_solution.
this
solver accessors to retrieve the last computed solution. [in]  dt  The time step used advance the solution in time. 
[in]  v_guess  The initial guess used in by the NewtonRaphson iteration. Typically, the previous time step velocities. 
std::logic_error  if v_guess is not of size nv , the number of generalized velocities specified at construction. 

friend 