Drake
Drake C++ Documentation
TamsiSolver< T > Class Template Reference

Detailed Description

template<typename T>
class drake::multibody::TamsiSolver< T >

TamsiSolver uses the Transition-Aware Modified Semi-Implicit (TAMSI) method, [Castro et al., 2019], to solve the equations below for mechanical systems in contact with regularized 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 TamsiSolver uses regularized 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).

TamsiSolver implements two different schemes. A "one-way 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 "two-way 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 two-way 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 one-way coupled scheme. The one-way coupled scheme is however very useful for testing and analysis of the solver.

One-Way Coupling Scheme

Equation (1) is discretized in time using a variation of the first order semi-implicit 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 Unicode-induced confusion with the "normal direction" subscript n.)

Please see details in the Derivation of the one-way 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 TamsiSolver 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.

Two-Way Coupling Scheme

Equation (1) is discretized in time using a variation on the semi-implicit 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 two-way 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 two-way 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 re-write 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.

TamsiSolver uses a Newton-Raphson strategy to solve Eq. (10) in the generalized velocities, limiting the iteration update with the scheme described in iteration_limiter.

Limits in the Iteration Updates

TamsiSolver solves for the generalized velocity at the next time step vˢ⁺¹ with either a one-way or two-way coupled scheme as described in the previous sections. The solver uses a Newton-Raphson iteration to compute an update Δvᵏ at the k-th Newton-Raphson 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. Details of our implementation are provided in [Castro et al., 2019].

Derivation of the one-way coupling scheme

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 semi-implicit 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 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 semi-implicit 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 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).

References

  • [Castro et al., 2019] Castro, A.M, Qu, A., Kuppuswamy, N., Alspach, A., Sherman, M.A., 2019. A Transition-Aware Method for the Simulation of Compliant Contact with Regularized Friction. arXiv:1909.05700 [cs.RO].
  • 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.
Template Parameters
TThe scalar type, which must be one of the default scalars.
Authors
Alejandro Castro (2018) Original author.
Michael Sherman, Evan Drumwright (2018) Original PR #8925 reviewers.
Drake team (see https://drake.mit.edu/credits).

#include <drake/multibody/plant/tamsi_solver.h>

Public Member Functions

 TamsiSolver (int nv)
 Instantiates a solver for a problem with nv generalized velocities. More...
 
void ResizeIfNeeded (int nv) const
 Change the working size of the solver to use nv generalized velocities. More...
 
std::unique_ptr< TamsiSolver< T > > Clone () const
 
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 >> fn0, 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...
 
TamsiSolverResult SolveWithGuess (double dt, const VectorX< T > &v_guess) const
 Given an initial guess v_guess, this method uses a Newton-Raphson iteration to find a solution for the generalized velocities satisfying either Eq. More...
 
const TamsiSolverIterationStatsget_iteration_statistics () const
 Returns statistics recorded during the last call to SolveWithGuess(). More...
 
const TamsiSolverParametersget_solver_parameters () const
 Returns the current set of parameters controlling the iteration process. More...
 
void set_solver_parameters (const TamsiSolverParameters &parameters)
 Sets the parameters to be used by the solver. More...
 
Does not allow copy, move, or assignment
 TamsiSolver (const TamsiSolver &)=delete
 
TamsiSolveroperator= (const TamsiSolver &)=delete
 
 TamsiSolver (TamsiSolver &&)=delete
 
TamsiSolveroperator= (TamsiSolver &&)=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 TamsiSolverTester
 

Constructor & Destructor Documentation

◆ TamsiSolver() [1/3]

TamsiSolver ( const TamsiSolver< T > &  )
delete

◆ TamsiSolver() [2/3]

TamsiSolver ( TamsiSolver< T > &&  )
delete

◆ TamsiSolver() [3/3]

TamsiSolver ( int  nv)
explicit

Instantiates a solver for a problem with nv generalized velocities.

Exceptions
std::exceptionif nv is non-positive.

Member Function Documentation

◆ Clone()

std::unique_ptr<TamsiSolver<T> > Clone ( ) const
Returns
a deep copy of this, with the problem data invalidated, i.e., one of the Set*ProblemData() methods must be called on the clone before it can be used to solve.

◆ get_friction_forces()

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.

◆ get_generalized_contact_forces()

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.

◆ get_generalized_friction_forces()

const VectorX<T>& get_generalized_friction_forces ( ) const

Returns a constant reference to the most recent vector of generalized friction forces.

◆ get_generalized_velocities()

const VectorX<T>& get_generalized_velocities ( ) const

Returns a constant reference to the most recent vector of generalized velocities.

◆ get_iteration_statistics()

const TamsiSolverIterationStats& get_iteration_statistics ( ) const

Returns statistics recorded during the last call to SolveWithGuess().

See IterationStats for details.

◆ get_normal_forces()

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.

◆ get_normal_velocities()

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.

◆ get_solver_parameters()

const TamsiSolverParameters& get_solver_parameters ( ) const

Returns the current set of parameters controlling the iteration process.

See Parameters for details.

◆ get_tangential_velocities()

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.

◆ operator=() [1/2]

TamsiSolver& operator= ( const TamsiSolver< T > &  )
delete

◆ operator=() [2/2]

TamsiSolver& operator= ( TamsiSolver< T > &&  )
delete

◆ ResizeIfNeeded()

void ResizeIfNeeded ( int  nv) const

Change the working size of the solver to use nv generalized velocities.

This can be used to either shrink or grow the workspaces.

Exceptions
std::exceptionif nv is non-positive.

◆ set_solver_parameters()

void set_solver_parameters ( const TamsiSolverParameters parameters)

Sets the parameters to be used by the solver.

See Parameters for details.

◆ SetOneWayCoupledProblemData()

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.

Parameters
[in]MThe mass matrix of the system, of size nv x nv.
[in]JnThe normal separation velocities Jacobian, of size nc x nv.
[in]JtThe tangential velocities Jacobian, of size 2nc x nv.
[in]p_starThe generalized momentum the system would have at s + 1 if contact forces were zero.
[in]fnA vector of size nc containing the normal force at each contact point.
[in]muA 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.
Warning
This method stores constant references to the matrices and vectors passed as arguments. Therefore
  1. they must outlive this class and,
  2. changes to the problem data invalidate any solution performed by this solver. In such a case, SetOneWayCoupledProblemData() and SolveWithGuess() must be invoked again.
Exceptions
std::exceptionif any of the data pointers are nullptr.
std::exceptionif the problem data sizes are not consistent as described above.
std::exceptionif SetTwoWayCoupledProblemData() was ever called on this solver.

◆ SetTwoWayCoupledProblemData()

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 >>  fn0,
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 two-way 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.

Parameters
[in]MThe mass matrix of the system, of size nv x nv.
[in]JnThe normal separation velocities Jacobian, of size nc x nv.
[in]JtThe tangential velocities Jacobian, of size 2nc x nv.
[in]p_starThe generalized momentum the system would have at n + 1 if contact forces were zero.
[in]fn0Normal force at the previous time step. Always positive since bodies cannot attract each other.
[in]stiffnessA vector of size nc storing at each ith entry the stiffness coefficient for the ith contact pair.
[in]dissipationA vector of size nc storing at each ith entry the dissipation coefficient for the ith contact pair.
[in]muA 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.
Warning
This method stores constant references to the matrices and vectors passed as arguments. Therefore
  1. they must outlive this class and,
  2. changes to the problem data invalidate any solution performed by this solver. In such a case, SetOneWayCoupledProblemData() and SolveWithGuess() must be invoked again.
Exceptions
std::exceptionif any of the data pointers are nullptr.
std::exceptionif the problem data sizes are not consistent as described above.
std::exceptionif SetOneWayCoupledProblemData() was ever called on this solver.

◆ SolveWithGuess()

TamsiSolverResult SolveWithGuess ( double  dt,
const VectorX< T > &  v_guess 
) const

Given an initial guess v_guess, this method uses a Newton-Raphson iteration to find a solution for the generalized velocities satisfying either Eq.

(3) when one-way coupling is used or Eq. (10) when two-way coupling is used. See this class's documentation for further details. To retrieve the solution, please refer to retrieving_the_solution.

Returns
kSuccess if the iteration converges. All other values of TamsiSolverResult report different failure modes. Uses this solver accessors to retrieve the last computed solution.
Warning
Always verify that the return value indicates success before retrieving the computed solution.
Parameters
[in]dtThe time step used advance the solution in time.
[in]v_guessThe initial guess used in by the Newton-Raphson iteration. Typically, the previous time step velocities.
Exceptions
std::exceptionif v_guess is not of size nv, the number of generalized velocities specified at construction.

Friends And Related Function Documentation

◆ TamsiSolverTester

friend class TamsiSolverTester
friend

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